[DO NOT MERGE] Test branch for real hrp2

hrpsys_ros_bridge/CMakeLists.txt

@@ -102,11 +102,11 @@ rtmbuild_genbridge()
 # pr2_controller_msgs is not catkinized
 string(RANDOM _random_string)
 
-rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp)
+rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp src/HrpsysROSBridgeUtil.cpp)
 rtmbuild_add_executable(ImageSensorROSBridge src/ImageSensorROSBridge.cpp src/ImageSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(RangeSensorROSBridge src/RangeSensorROSBridge.cpp src/RangeSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(PointCloudROSBridge src/PointCloudROSBridge.cpp src/PointCloudROSBridgeComp.cpp)
-rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp)
+rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp src/HrpsysROSBridgeUtil.cpp)
 
 install(DIRECTORY launch euslisp srv idl scripts models test cmake
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l

@@ -185,12 +185,14 @@
   (:motor-extra-data
    ()
    "Returns motor extra data. Please see iob definition for each system."
-   (let* ((d (cdr (assoc :motor-extra-data robot-state)))
-          (dims (mapcar #'(lambda (x) (send x :size)) (send (send d :layout) :dim)))
-          (ret))
-     (dotimes (j (car dims))
-       (push (subseq (send d :data) (* j (cadr dims)) (* (1+ j) (cadr dims))) ret))
-     (reverse ret)))
+   (if (cdr (assoc :motor-extra-data robot-state))
+       (let* ((d (cdr (assoc :motor-extra-data robot-state)))
+              (dims (mapcar #'(lambda (x) (send x :size)) (send (send d :layout) :dim)))
+              (ret))
+         (dotimes (j (car dims))
+           (push (subseq (send d :data) (* j (cadr dims)) (* (1+ j) (cadr dims))) ret))
+         (reverse ret))
+     nil))
   (:imucoords
    ()
    "Returns robot's coords based on imu measurement."
@@ -329,9 +331,13 @@
         (debug nil))
   (when (boundp param-class)
     (let* ((param-slots-list ;; get slots list for param-class
-            (remove-if #'(lambda (x) (or (string= "plist" x) (string= "connection-header" x)))
-                       (mapcar #'(lambda (x) (string-left-trim "::_" (string-left-trim "ros" (format nil "~A" x))))
-                               (concatenate cons (send (eval param-class) :slots)))))
+            ;; coerce to cons to avoid :count glitches on vectors
+            ;; https://github.com/euslisp/EusLisp/issues/295
+            (let ((slt (coerce (send (eval param-class) :slots) cons)))
+              (map nil #'(lambda (obj) (setq slt (remove obj slt :count 1)))
+                   (send ros::object :slots))
+              (mapcar #'(lambda (x) (string-left-trim "_" (symbol-name x)))
+                      slt)))
            (param-name
             ;; Extract param class, e.g., extract zz from hrpsys_ros_bridge::xx_yy_zz
             (car (last (read-from-string (substitute (elt " " 0) (elt ":" 0) (substitute (elt " " 0) (elt "_" 0) (format nil "(~A)" param-class)))))))
@@ -500,6 +506,26 @@
    (if update-robot-state (send self :state))
    (send self :set-ref-forces-moments (send self :reference-force-vector) moment-list tm)
    )
+  (:set-ref-force-moment
+   (force moment tm &optional (limb :arms) &key (update-robot-state t))
+   "Set reference force [N] and moment [Nm]. tm is interpolation time [ms].
+    limb should be limb symbol name such as :rarm, :larm, :rleg, :lleg, :arms, or :legs."
+   (if update-robot-state (send self :state))
+   (let ((limbs (case limb
+		  (:arms (list :rarm :larm))
+		  (:legs (list :rleg :lleg))
+		  (t (list limb)))))
+     (send self :set-ref-forces-moments
+           (mapcar #'(lambda (fs rfv)
+                       (if (find-if #'(lambda (l) (equal fs (car (send robot l :force-sensors)))) limbs)
+                           force rfv))
+                   (send robot :force-sensors) (send self :reference-force-vector))
+	   (mapcar #'(lambda (fs rfv)
+                       (if (find-if #'(lambda (l) (equal fs (car (send robot l :force-sensors)))) limbs)
+                           moment rfv))
+                   (send robot :force-sensors) (send self :reference-moment-vector))
+           tm)
+     ))
   (:set-ref-force
    (force tm &optional (limb :arms) &key (update-robot-state t))
    "Set reference force [N]. tm is interpolation time [ms].

hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

@@ -24,13 +24,14 @@
   <arg name="USE_SERVOCONTROLLER" default="false" />
   <arg name="USE_TORQUECONTROLLER" default="false" />
   <arg name="USE_SOFTERRORLIMIT" default="true" />
+  <arg name="USE_THERMOESTIMATOR" default="true" />
   <arg name="USE_TORQUEFILTER" default="false" />
   <arg name="USE_IMAGESENSOR" default="false" />
   <arg name="USE_EMERGENCYSTOPPER" default="false" />
   <arg name="USE_REFERENCEFORCEUPDATER" default="false" />
   <arg name="USE_OBJECTCONTACTTURNAROUNDDETECTOR" default="false" />
   <arg name="USE_HRPSYS_PROFILE" default="true" />
-  <arg name="USE_VELOCITY_OUTPUT" default="false" />
+  <arg name="USE_VELOCITY_OUTPUT" default="false" doc="true: output actual velocity / false: output differential value of actual postition as /joint_states/velocity"/>
   <arg name="PUBLISH_SENSOR_TF" default="true" />
   <!--
       roslaunch hrpsys_ros_bridge sample.launch nameserver:=192.168.1.1
@@ -97,7 +98,7 @@
   <env name="push_policy" value="$(arg push_policy)" />
   <env name="push_rate" value="$(arg push_rate)" />
   <env name="buffer_length" value="$(arg buffer_length)" />
-  <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch USE_COMMON=$(arg USE_COMMON) USE_WALKING=$(arg USE_WALKING) USE_COLLISIONCHECK=$(arg USE_COLLISIONCHECK) USE_IMPEDANCECONTROLLER=$(arg USE_IMPEDANCECONTROLLER) USE_SOFTERRORLIMIT=$(arg USE_SOFTERRORLIMIT) USE_IMAGESENSOR=$(arg USE_IMAGESENSOR) USE_ROBOTHARDWARE=$(arg USE_ROBOTHARDWARE) USE_GRASPCONTROLLER=$(arg USE_GRASPCONTROLLER) USE_SERVOCONTROLLER=$(arg USE_SERVOCONTROLLER) USE_TORQUECONTROLLER=$(arg USE_TORQUECONTROLLER) USE_TORQUEFILTER=$(arg USE_TORQUEFILTER) USE_EMERGENCYSTOPPER=$(arg USE_EMERGENCYSTOPPER) USE_REFERENCEFORCEUPDATER=$(arg USE_REFERENCEFORCEUPDATER) USE_OBJECTCONTACTTURNAROUNDDETECTOR=$(arg USE_OBJECTCONTACTTURNAROUNDDETECTOR) USE_VELOCITY_OUTPUT=$(arg USE_VELOCITY_OUTPUT)" output="$(arg OUTPUT)"/>
+  <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch USE_COMMON=$(arg USE_COMMON) USE_WALKING=$(arg USE_WALKING) USE_COLLISIONCHECK=$(arg USE_COLLISIONCHECK) USE_IMPEDANCECONTROLLER=$(arg USE_IMPEDANCECONTROLLER) USE_SOFTERRORLIMIT=$(arg USE_SOFTERRORLIMIT) USE_IMAGESENSOR=$(arg USE_IMAGESENSOR) USE_ROBOTHARDWARE=$(arg USE_ROBOTHARDWARE) USE_GRASPCONTROLLER=$(arg USE_GRASPCONTROLLER) USE_SERVOCONTROLLER=$(arg USE_SERVOCONTROLLER) USE_TORQUECONTROLLER=$(arg USE_TORQUECONTROLLER) USE_THERMOESTIMATOR=$(arg USE_THERMOESTIMATOR) USE_TORQUEFILTER=$(arg USE_TORQUEFILTER) USE_EMERGENCYSTOPPER=$(arg USE_EMERGENCYSTOPPER) USE_REFERENCEFORCEUPDATER=$(arg USE_REFERENCEFORCEUPDATER) USE_OBJECTCONTACTTURNAROUNDDETECTOR=$(arg USE_OBJECTCONTACTTURNAROUNDDETECTOR) USE_VELOCITY_OUTPUT=$(arg USE_VELOCITY_OUTPUT)" output="$(arg OUTPUT)"/>
 
 
   <!-- All Robots -->
@@ -169,6 +170,7 @@
     <rtconnect from="StabilizerServiceROSBridge.rtc:StabilizerService" to="st.rtc:StabilizerService"  subscription_type="new"/>
     <rtconnect from="KalmanFilterServiceROSBridge.rtc:KalmanFilterService" to="kf.rtc:KalmanFilterService"  subscription_type="new"/>
     <rtconnect from="st.rtc:COPInfo" to="HrpsysSeqStateROSBridge0.rtc:rsCOPInfo" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="HrpsysSeqStateROSBridge0.rtc:SegwaySensors" to="st.rtc:SegwaySensors" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
     <rtactivate component="AutoBalancerServiceROSBridge.rtc" />
     <rtactivate component="StabilizerServiceROSBridge.rtc" />
     <rtactivate component="KalmanFilterServiceROSBridge.rtc" />
@@ -251,6 +253,16 @@
     <rtactivate component="SoftErrorLimiterServiceROSBridge.rtc" />
   </group>
 
+  <!-- USE_THERMOESTIMATOR
+       ThermoEstimator
+  -->
+  <group if="$(arg USE_THERMOESTIMATOR)" >
+    <node pkg="hrpsys_ros_bridge" name="ThermoEstimatorServiceROSBridge" type="ThermoEstimatorServiceROSBridgeComp"
+          output="screen" args ="$(arg openrtm_args)" />
+    <rtconnect from="ThermoEstimatorServiceROSBridge.rtc:ThermoEstimatorService" to="te.rtc:ThermoEstimatorService"  subscription_type="new"/>
+    <rtactivate component="ThermoEstimatorServiceROSBridge.rtc" />
+  </group>
+
   <!-- USE_EMERGENCYSTOPPER
        EmergencyStopper
   -->

hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp

@@ -41,6 +41,9 @@
 #include "hrpsys_ros_bridge/idl/ExecutionProfileService.hh"
 #include "hrpsys_ros_bridge/idl/RobotHardwareService.hh"
 
+// Version
+#include "HrpsysROSBridgeUtil.h"
+
 // Module specification
 // <rtc-template block="module_spec">
 static const char* hrpsysjointtrajectorybridge_spec[] = {"implementation_id", "HrpsysJointTrajectoryBridge",
@@ -186,6 +189,8 @@ RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onActivated(RTC::UniqueId ec_id)
     ROS_WARN_STREAM("param: " << config_name << ", param does not exist.");
   }
 
+  hrpsys_version = GetHrpsysVersion(m_SequencePlayerServicePort);
+
   return RTC::RTC_OK;
 }
 
@@ -330,6 +335,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
 
   // FIXME: need to set actual informatoin, currently we set dummy information
   trajectory_msgs::JointTrajectoryPoint commanded_joint_trajectory_point, error_joint_trajectory_point;
+  commanded_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   commanded_joint_trajectory_point.positions.resize(joint_list.size());
   commanded_joint_trajectory_point.velocities.resize(joint_list.size());
   commanded_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -341,6 +347,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
       commanded_joint_trajectory_point.accelerations[j] = parent->body->link(joint_list[j])->ddq;
       commanded_joint_trajectory_point.effort[j]        = parent->body->link(joint_list[j])->u;
     }
+  error_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   error_joint_trajectory_point.positions.resize(joint_list.size());
   error_joint_trajectory_point.velocities.resize(joint_list.size());
   error_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -462,8 +469,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       ss << " " << point.positions[j];
     }
     ROS_INFO_STREAM(
-        "[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : time_from_start " << trajectory.points[i].time_from_start.toSec());
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] " << ss.str());
+        "[" << parent->getInstanceName() << "] i:" << i << " : time_from_start " << trajectory.points[i].time_from_start.toSec());
 
     if (i > 0)
     {
@@ -474,6 +480,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       duration[i] = trajectory.points[i].time_from_start.toSec();
       if ( std::abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set magic delta ... https://github.com/start-jsk/rtmros_common/issues/1036
     }
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] [" << ss.str() << "] " << duration[i]);
   }
 
   parent->m_mutex.unlock();
@@ -486,22 +493,38 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     }
     else
     { // fullbody
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAngles");
       parent->m_service0->setJointAngles(angles[0], duration[0]);
     }
   }
   else
   {
     if (groupname.length() > 0)
     { // group
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternGroup: " << groupname);
-      parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternOfGroup: " << groupname);
+        parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesSequenceOfGroup: " << groupname);
+        parent->m_service0->setJointAnglesSequenceOfGroup(groupname.c_str(), angles, duration);
+      }
     }
     else
-    {
-      OpenHRP::dSequenceSequence rpy, zmp;
-      parent->m_service0->playPattern(angles, rpy, zmp, duration);
+    { // fullbody
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPattern");
+        OpenHRP::dSequenceSequence rpy, zmp;
+        parent->m_service0->playPattern(angles, rpy, zmp, duration);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesSequence");
+        parent->m_service0->setJointAnglesSequence(angles, duration);
+      }
     }
   }
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -527,13 +550,67 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
   joint_trajectory_server->setPreempted();
+  if (!joint_trajectory_server->isNewGoalAvailable())
+  { // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes
+    if (groupname.length() > 0)
+    { // group
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
+        parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+        parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+      }
+    }
+    else
+    { // fullbody
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
+        parent->m_service0->clear();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
+      }
+    }
+  }
 }
 #endif
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTrajectoryActionPreempt()
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionPreempt()");
   follow_joint_trajectory_server->setPreempted();
+
+  if (!follow_joint_trajectory_server->isNewGoalAvailable())
+  { // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes
+    if (groupname.length() > 0)
+    { // group
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
+        parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+        parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+      }
+    }
+    else
+    { // fullbody
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
+        parent->m_service0->clear();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
+      }
+    }
+  }
 }
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg)

hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h

@@ -102,6 +102,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
     std::string groupname;
     std::vector<std::string> joint_list;
     bool interpolationp;
+    ros::Time traj_start_tm;
 
   public:
     typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;
@@ -126,6 +127,8 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
   RTC::CorbaPort m_SequencePlayerServicePort;
   RTC::CorbaConsumer<OpenHRP::SequencePlayerService> m_service0;
 
+  std::string hrpsys_version;
+
 protected:
   hrp::BodyPtr body;
   OpenHRP::BodyInfo_var bodyinfo;

hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp

@@ -0,0 +1,70 @@
+// -*- C++ -*-
+/*!
+ * @file  HrpsysROSBridgeUtil.cpp * @brief hrpsys - ros bridge util files * @date  $Date$ 
+ *
+ * $Id$ 
+ */
+
+#include "HrpsysROSBridgeUtil.h"
+#include <ros/console.h>
+
+//
+void Parse(int result[3], const std::string& input)
+{
+  std::istringstream parser(input);
+  parser >> result[0];
+  for(int idx = 1; idx < 3; idx++)
+  {
+    parser.get(); //Skip period
+    parser >> result[idx];
+  }
+}
+
+bool LessThanVersion(const std::string& a,const std::string& b)
+{
+  int parsedA[3], parsedB[3];
+  try {
+    Parse(parsedA, a);
+    Parse(parsedB, b);
+    return std::lexicographical_compare(parsedA, parsedA + 4, parsedB, parsedB + 4);
+  } catch (...) {
+    ROS_ERROR_STREAM("LessThanVersion failed " << a << " < " << b <<", force return false");
+    return false;
+  }
+}
+
+//
+std::string GetHrpsysVersion(RTC::CorbaPort& m_ServicePort) {
+  std::string hrpsys_version;
+  try {
+    // copied from HrpsysSeqStateROSBridgeImpl.cpp
+    RTC::ConnectorProfileList* connector_profile_list = m_ServicePort.get_connector_profiles();
+    for (int i = 0; i < connector_profile_list->length(); i++ ) {
+      RTC::ConnectorProfile& connector_profile = (*connector_profile_list)[i];
+      RTC::PortServiceList ports_list = connector_profile.ports;
+      for (int j = 0; j < ports_list.length(); j++ ) {
+        RTC::PortService_var port = ports_list[j];
+        RTC::PortProfile* profile = port->get_port_profile();
+        if ( profile ) {
+          RTC::RTObject_var owner = profile->owner;
+          if ( owner ) {
+            RTC::ComponentProfile* component_profile = owner->get_component_profile();
+            if ( component_profile && std::string(component_profile->type_name) == std::string("SequencePlayer") ) {
+              ROS_WARN_STREAM(__PRETTY_FUNCTION__ << " Connected to " << component_profile->type_name  << " (" << component_profile->instance_name << ") version " << component_profile->version);
+              hrpsys_version = std::string(component_profile->version);
+            }
+          }
+        }
+      }
+    }
+    if ( hrpsys_version == "" ) {
+      ROS_ERROR_STREAM( __PRETTY_FUNCTION__ << " Could not get valid hrpsys_version");
+    }
+  } catch (...) {
+    ROS_ERROR_STREAM( __PRETTY_FUNCTION__ << " Could not get hrpsys_version");
+  }
+  return hrpsys_version;
+}
+
+
+