[WIP] 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/launch/hrpsys_ros_bridge.launch

@@ -30,7 +30,7 @@
   <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

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;
+}
+
+
+

hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h

@@ -0,0 +1,31 @@
+// -*- C++ -*-
+/*!
+ * @file  HrpsysROSBridgeUtil.h * @brief hrpsys - ros bridge util files * @date  $Date$ 
+ *
+ * $Id$ 
+ */
+#ifndef HRPSYSROSBRIDGEUTIL_H
+#define HRPSYSROSBRIDGEUTIL_H
+
+#include <string>
+// http://stackoverflow.com/questions/2941491/compare-versions-as-strings
+bool LessThanVersion(const std::string& a,const std::string& b);
+
+//
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <rtm/Manager.h>
+#include <rtm/CorbaNaming.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <string>
+#include <iostream>
+
+std::string GetHrpsysVersion(RTC::CorbaPort& m_ServicePort);
+
+#endif // HRPSYSROSBRIDGEUTIL_H
+

hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

@@ -215,9 +215,15 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
   if ( duration.length() == 1 ) {
     m_service0->setJointAngles(angles[0], duration[0]);
   } else {
-    OpenHRP::dSequenceSequence rpy, zmp;
-    m_service0->playPattern(angles, rpy, zmp, duration);
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      OpenHRP::dSequenceSequence rpy, zmp;
+      m_service0->playPattern(angles, rpy, zmp, duration);
+    } else {
+      m_service0->setJointAnglesSequence(angles, duration);
+    }
   }
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -238,11 +244,31 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
 #ifdef USE_PR2_CONTROLLERS_MSGS
 void HrpsysSeqStateROSBridge::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.
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      m_service0->clear();
+    } else {
+      m_service0->clearJointAngles();
+    }
+  }
 }
 #endif
 
 void HrpsysSeqStateROSBridge::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.
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      m_service0->clear();
+    } else {
+      m_service0->clearJointAngles();
+    }
+  }
 }
 
 void HrpsysSeqStateROSBridge::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg) {
@@ -442,6 +468,9 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         //joint_state.velocity
         //joint_state.effort
         follow_joint_trajectory_feedback.joint_names.push_back(j->name);
+        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - traj_start_tm;
         follow_joint_trajectory_feedback.desired.positions.push_back(j->q);
         follow_joint_trajectory_feedback.actual.positions.push_back(j->q);
         follow_joint_trajectory_feedback.error.positions.push_back(0);
@@ -463,8 +492,16 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         joint_state.velocity.push_back(m_rsvel.data[i]);
       }
     } else {
-      joint_state.velocity.resize(joint_state.name.size());
+      double time_from_prev = (joint_state.header.stamp - prev_joint_state.header.stamp).toSec();
+      if(time_from_prev > 0 && prev_joint_state.position.size() == joint_state.position.size()) {
+        for (unsigned int i = 0; i < joint_state.position.size(); i++) {
+          joint_state.velocity.push_back((joint_state.position[i] - prev_joint_state.position[i]) / time_from_prev);
+        }
+      } else {
+        joint_state.velocity.resize(joint_state.name.size());
+      }
     }
+    prev_joint_state = joint_state;
     // set effort if m_rstorque is available
     if (m_rstorque.data.length() == body->joints().size()) {
       for ( unsigned int i = 0; i < body->joints().size() ; i++ ){
@@ -574,7 +611,8 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
     }
     if ( !follow_joint_trajectory_feedback.joint_names.empty() &&
          !follow_joint_trajectory_feedback.actual.positions.empty() &&
-         follow_action_initialized ) {
+         follow_action_initialized &&
+         follow_joint_trajectory_server.isActive() ) {
       follow_joint_trajectory_server.publishFeedback(follow_joint_trajectory_feedback);
     }
   } // end: m_mcangleIn