Add joint limits to the diagnostics

.gitignore

@@ -3,3 +3,5 @@ build*
 doc/*
 !doc/source/*
 !doc/Doxyfile
+*.pro
+*.pro.user

src/converters/diagnostics.cpp

@@ -63,13 +63,51 @@ DiagnosticsConverter::DiagnosticsConverter( const std::string& name, float frequ
     p_body_temperature_.call<void>("setEnableNotifications", true);
   }
 
+  std::vector<std::vector<float> > joint_limits;
+  qi::AnyValue qi_joint_limits;
+
   // Get all the joint names
-  qi::AnyObject p_motion = session->service("ALMotion");
-  joint_names_ = p_motion.call<std::vector<std::string> >("getBodyNames", "JointActuators" );
+  this->p_motion_ = session->service("ALMotion");
+  joint_names_ = this->p_motion_.call<std::vector<std::string> >("getBodyNames", "JointActuators");
 
   for(std::vector<std::string>::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<qi::AnyValue>(
+                    "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<double>(joint_limits[0][0]));
+    this->joint_limit_map_[(*it)].push_back(
+                static_cast<double>(joint_limits[0][1]));
+    this->joint_limit_map_[(*it)].push_back(
+                static_cast<double>(joint_limits[0][2]));
+    this->joint_limit_map_[(*it)].push_back(
+                static_cast<double>(joint_limits[0][3]));
   }
 
   // Get all the battery keys
@@ -125,6 +163,10 @@ void DiagnosticsConverter::callAll( const std::vector<message_actions::MessageAc
     status.hardware_id = joint_names_[i];
     status.add("Temperature", temperature);
     status.add("Stiffness", stiffness);
+    status.add("minAngle", this->joint_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<message_actions::MessageAc
     else
       ss << "Total Current: " << std::setw(5) << current << " Ampere (discharging)";
     status.message = ss.str();
-
     msg.status.push_back(status);
   }
 

src/converters/diagnostics.hpp

@@ -60,9 +60,12 @@ class DiagnosticsConverter : public BaseConverter<DiagnosticsConverter>
   std::vector<std::string> all_keys_;
   /** Keys for the battery status */
   std::vector<std::string> battery_status_keys_;
-
+  /** Map storing the joints informations */
+  std::map<std::string, std::vector<double> > 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_;
 

src/tools/from_any_value.cpp

@@ -218,5 +218,44 @@ std::vector<std::string> fromAnyValueToStringVector(qi::AnyValue& value, std::ve
   return result;
 }
 
+
+void fromAnyValueToFloatVectorVector(
+        qi::AnyValue &value,
+        std::vector< std::vector<float> > &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<anyrefs.size(); i++) {
+    qi::AnyReferenceVector anyref;
+
+    try {
+      anyref = anyrefs[i].asListValuePtr();
+
+    } catch (const std::exception& e) {
+      throw std::exception(e);
+    }
+
+    result[i].resize(anyref.size());
+
+    for(int j=0; j<anyref.size(); j++) {
+      try {
+        result[i][j] = anyref[j].content().toFloat();
+
+      } catch(std::runtime_error& e) {
+        throw std::exception(e);
+      }
+    }
+
+  }
+}
 }
 }

src/tools/from_any_value.hpp

@@ -38,8 +38,11 @@ std::vector<std::string> fromAnyValueToStringVector(qi::AnyValue& value, std::ve
 
 std::vector<float> fromAnyValueToFloatVector(qi::AnyValue& value, std::vector<float>& result);
 
-}
+void fromAnyValueToFloatVectorVector(
+        qi::AnyValue &value,
+        std::vector< std::vector<float> > &result);
 
 }
+}
 
 #endif // FROM_ANY_VALUE_HPP