use setJointAnglesOfGroup() and playPattern(), use clearJointAngles()

pazeshun · Nov 30, 2019 · 757d500 · 757d500
1 parent 0877b3d
commit 757d500
Show file tree

Hide file tree

Showing 4 changed files with 83 additions and 7 deletions.
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/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;
 }
 
@@ -493,13 +498,23 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
   {
     if (groupname.length() > 0)
     { // group
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternGroup: " << groupname);
-      parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesOfGroup: " << groupname);
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      } else {
+        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"))) {
+        OpenHRP::dSequenceSequence rpy, zmp;
+        parent->m_service0->playPattern(angles, rpy, zmp, duration);
+      } else {
+        parent->m_service0->setJointAnglesSequence(angles, duration);
+      }
     }
   }
 
@@ -527,13 +542,52 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
   joint_trajectory_server->setPreempted();
+  if (groupname.length() > 0)
+  { // group
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+    } else {
+      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"))) {
+      parent->m_service0->clear();
+    } else {
+      parent->m_service0->clearJointAngles();
+    }
+  }
 }
 #endif
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTrajectoryActionPreempt()
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionPreempt()");
   follow_joint_trajectory_server->setPreempted();
+
+  if (groupname.length() > 0)
+  { // group
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+    } else {
+      parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+    }
+  }
+  else
+  { // fullbody
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clearJointAngles();
+    } else {
+      parent->m_service0->clear();
+    }
+  }
 }
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
@@ -126,6 +126,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;

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -215,8 +215,13 @@ 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);
+    }
   }
 
   interpolationp = true;
@@ -238,11 +243,23 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
 #ifdef USE_PR2_CONTROLLERS_MSGS
 void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
   joint_trajectory_server.setPreempted();
+  // 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();
+  // 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) {

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
@@ -36,6 +36,9 @@
 
 // </rtc-template>
 
+// version
+#include "HrpsysROSBridgeUtil.h"
+
 #include "tf/transform_broadcaster.h"
 using namespace RTC;