diff --git a/.gitignore b/.gitignore index dd6c2a57..ea4a51c7 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ build* doc/* !doc/source/* !doc/Doxyfile +*.pro +*.pro.user diff --git a/src/converters/diagnostics.cpp b/src/converters/diagnostics.cpp index 5d7d5769..539ef8e0 100644 --- a/src/converters/diagnostics.cpp +++ b/src/converters/diagnostics.cpp @@ -63,13 +63,51 @@ DiagnosticsConverter::DiagnosticsConverter( const std::string& name, float frequ p_body_temperature_.call("setEnableNotifications", true); } + std::vector > joint_limits; + qi::AnyValue qi_joint_limits; + // Get all the joint names - qi::AnyObject p_motion = session->service("ALMotion"); - joint_names_ = p_motion.call >("getBodyNames", "JointActuators" ); + this->p_motion_ = session->service("ALMotion"); + joint_names_ = this->p_motion_.call >("getBodyNames", "JointActuators"); for(std::vector::const_iterator it = joint_names_.begin(); it != joint_names_.end(); ++it) { all_keys_.push_back(std::string("Device/SubDeviceList/") + (*it) + std::string("/Temperature/Sensor/Value")); all_keys_.push_back(std::string("Device/SubDeviceList/") + (*it) + std::string("/Hardness/Actuator/Value")); + + // Get the joint limits + joint_limits.clear(); + + try { + qi_joint_limits = this->p_motion_.call( + "getLimits", + (*it)); + + } catch (const std::exception &e) { + std::cerr << "Exception caught in DiagnosticsConverter: " + << e.what() + << std::endl; + continue; + } + + try { + tools::fromAnyValueToFloatVectorVector(qi_joint_limits, joint_limits); + + } catch (std::exception &e) { + std::cerr << "Error while converting the qi value corresponding to " + << "the joint's limits : " + << e.what() + << std::endl; + continue; + } + + this->joint_limit_map_[(*it)].push_back( + static_cast(joint_limits[0][0])); + this->joint_limit_map_[(*it)].push_back( + static_cast(joint_limits[0][1])); + this->joint_limit_map_[(*it)].push_back( + static_cast(joint_limits[0][2])); + this->joint_limit_map_[(*it)].push_back( + static_cast(joint_limits[0][3])); } // Get all the battery keys @@ -125,6 +163,10 @@ void DiagnosticsConverter::callAll( const std::vectorjoint_limit_map_[joint_names_[i]][0]); + status.add("maxAngle", this->joint_limit_map_[joint_names_[i]][1]); + status.add("maxVelocity", this->joint_limit_map_[joint_names_[i]][2]); + status.add("maxTorque", this->joint_limit_map_[joint_names_[i]][3]); // Define the level if (temperature < temperature_warn_level_) @@ -241,7 +283,6 @@ void DiagnosticsConverter::callAll( const std::vector std::vector all_keys_; /** Keys for the battery status */ std::vector battery_status_keys_; - + /** Map storing the joints informations */ + std::map > joint_limit_map_; /** Proxy to ALMemory */ qi::AnyObject p_memory_; + /** Proxy to ALMotion */ + qi::AnyObject p_motion_; /** Proxy to ALBodyTemperature */ qi::AnyObject p_body_temperature_; diff --git a/src/tools/from_any_value.cpp b/src/tools/from_any_value.cpp index 960f6e7f..a0cce4c0 100644 --- a/src/tools/from_any_value.cpp +++ b/src/tools/from_any_value.cpp @@ -218,5 +218,44 @@ std::vector fromAnyValueToStringVector(qi::AnyValue& value, std::ve return result; } + +void fromAnyValueToFloatVectorVector( + qi::AnyValue &value, + std::vector< std::vector > &result) { + + qi::AnyReferenceVector anyrefs; + + try { + anyrefs = value.asListValuePtr(); + + } catch (const std::exception& e) { + throw std::exception(e); + } + + result.resize(anyrefs.size()); + + for(int i=0; i fromAnyValueToStringVector(qi::AnyValue& value, std::ve std::vector fromAnyValueToFloatVector(qi::AnyValue& value, std::vector& result); -} +void fromAnyValueToFloatVectorVector( + qi::AnyValue &value, + std::vector< std::vector > &result); } +} #endif // FROM_ANY_VALUE_HPP