Merge pull request #88 from jontje/add_external_axes_support

Add external axes support
ros-industrial · Mar 30, 2020 · c91601d · c91601d
2 parents 5efd20a + 726d4c0
commit c91601d
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 6 deletions.
diff --git a/include/abb_libegm/egm_common.h b/include/abb_libegm/egm_common.h
@@ -48,6 +48,7 @@ namespace egm
  */
 enum RobotAxes
 {
+  None  = 0, ///< \brief No robot axes are expected (i.e. only external axes).
   Six   = 6, ///< \brief A six axes robot.
   Seven = 7  ///< \brief A seven axes robot.
 };

diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp
@@ -391,7 +391,7 @@ void EGMBaseInterface::OutputContainer::constructReply(const BaseConfiguration&
   constructHeader();
   bool success = constructJointBody(configuration);
 
-  if (success)
+  if (success && configuration.axes != None)
   {
     success = constructCartesianBody(configuration);
   }
@@ -515,6 +515,20 @@ bool EGMBaseInterface::OutputContainer::constructJointBody(const BaseConfigurati
 
     switch (configuration.axes)
     {
+      case None:
+      {
+        if (robot_position.values_size() == 0)
+        {
+          for (int i = 0; i < external_position.values_size() && i < ext_condition; ++i)
+          {
+            planned->mutable_externaljoints()->add_joints(external_position.values(i));
+          }
+
+          position_ok = true;
+        }
+      }
+      break;
+
       case Six:
       {
         if (robot_position.values_size() == rob_condition)

diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp
@@ -728,6 +728,20 @@ bool parse(wrapper::Joints* p_target_robot,
 
     switch (axes)
     {
+      case None:
+      {
+        if (source_robot.joints_size() == 0)
+        {
+          for (int i = 0; i < source_external.joints_size(); ++i)
+          {
+            p_target_external->add_values(source_external.joints(i));
+          }
+
+          success = true;
+        }
+      }
+      break;
+
       case Six:
       {
         if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS)
@@ -842,7 +856,14 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx
 
     if (success)
     {
-      success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());
+      if(axes == None)
+      {
+        success = !source.has_cartesian();
+      }
+      else
+      {
+        success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());
+      }
 
       if (success)
       {
@@ -866,7 +887,14 @@ bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes
 
     if (success)
     {
-      success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());
+      if(axes == None)
+      {
+        success = !source.has_cartesian();
+      }
+      else
+      {
+        success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());
+      }
 
       if (success)
       {

diff --git a/src/egm_trajectory_interface.cpp b/src/egm_trajectory_interface.cpp
@@ -194,7 +194,7 @@ wrapper::trajectory::ExecutionProgress_SubState EGMTrajectoryInterface::Trajecto
 void EGMTrajectoryInterface::TrajectoryMotion::MotionStep::resetMotionStep()
 {
   unsigned int robot_joints = data.feedback.robot().joints().position().values_size();
-  unsigned int external_joints = data.feedback.robot().joints().position().values_size();
+  unsigned int external_joints = data.feedback.external().joints().position().values_size();
 
   internal_goal.Clear();
   external_goal.Clear();
@@ -243,7 +243,7 @@ void EGMTrajectoryInterface::TrajectoryMotion::MotionStep::resetMotionStep()
 void EGMTrajectoryInterface::TrajectoryMotion::MotionStep::prepareNormalGoal(const bool last_point)
 {
   unsigned int robot_joints = data.feedback.robot().joints().position().values_size();
-  unsigned int external_joints = data.feedback.robot().joints().position().values_size();
+  unsigned int external_joints = data.feedback.external().joints().position().values_size();
 
   data.mode = (external_goal.robot().has_cartesian() ? EGMPose : EGMJoint);
 
@@ -394,7 +394,7 @@ double EGMTrajectoryInterface::TrajectoryMotion::MotionStep::estimateDuration()
   double estimate = 0.0;
 
   unsigned int robot_joints = data.feedback.robot().joints().position().values_size();
-  unsigned int external_joints = data.feedback.robot().joints().position().values_size();
+  unsigned int external_joints = data.feedback.external().joints().position().values_size();
 
   // Reset robot joint values.
   reset(internal_goal.mutable_robot()->mutable_joints()->mutable_velocity(), robot_joints);