Two Arms and Rail With DX100

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h

@@ -202,6 +202,13 @@ class JointTrajectoryAction
 
   void watchdog(const ros::TimerEvent &e, int group_number);
 
+  /**
+   * \brief Check if all robot groups finished their trajectory.
+   *
+   * \return true if all robot groups finished their trajectory.
+   */
+  bool allGroupsFinished() const;
+
   /**
    * \brief Action server goal callback method
    *

motoman_driver/include/motoman_driver/industrial_robot_client/robot_state_interface.h

@@ -165,7 +165,8 @@ class RobotStateInterface
    */
   void add_handler(MessageHandler* handler, bool allow_replace = true)
   {
-    this->manager_.add(handler, allow_replace);
+    if (!this->manager_.add(handler, allow_replace))
+      ROS_WARN_STREAM("Failed to add handler!!!!");
   }
 
 protected:

motoman_driver/launch/robot_interface_streaming.launch

@@ -39,7 +39,7 @@
 	<!-- motion_streaming_interface: sends robot motion commands by 
 	     STREAMING path to robot (using socket connection to robot) 
 	-->
-	<include file="$(find motoman_driver)/launch/motion_streaming_interface.launch">
+  <include file="$(find motoman_driver)/launch/motion_streaming_interface.launch">
 		<arg name="robot_ip"   value="$(arg robot_ip)" />
 		<arg name="use_bswap"  value="$(arg use_bswap)" />
 	</include>

motoman_driver/launch/robot_interface_streaming_dx100.launch

@@ -15,6 +15,6 @@
 	<include file="$(find motoman_driver)/launch/robot_interface_streaming.launch">
 		<arg name="robot_ip"   value="$(arg robot_ip)" />
 		<arg name="use_bswap"  value="$(arg use_bswap)" />
-		<arg name="version0"  value="$(arg version0)" />
+    <arg name="version0"  value="false" />
 	</include>
 </launch>

motoman_driver/launch/robot_state.launch

@@ -13,7 +13,7 @@
 
 	<!-- load the correct version of the robot state node -->
 	<node if="$(arg use_bswap)" name="joint_state" 
-		pkg="motoman_driver" type="robot_state_bswap" />
+		pkg="motoman_driver" type="robot_state_bswap" output="screen"/>
 	<node unless="$(arg use_bswap)" name="joint_state" 
-		pkg="motoman_driver" type="robot_state" />
+		pkg="motoman_driver" type="robot_state" output="screen"/>
 </launch>

motoman_driver/src/industrial_robot_client/joint_feedback_ex_relay_handler.cpp

@@ -153,7 +153,7 @@ bool JointFeedbackExRelayHandler::create_messages(JointFeedbackMessage& msg_in,
   sensor_state->position = pub_joint_state.positions;
   sensor_state->velocity = pub_joint_state.velocities;
 
-  this->pub_joint_sensor_state_.publish(*sensor_state);
+//  this->pub_joint_sensor_state_.publish(*sensor_state);
 
   return true;
 }

motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp

@@ -1,4 +1,5 @@
 /*
+ *
  * Software License Agreement (BSD License)
  *
  * Copyright (c) 2014, Fraunhofer IPA
@@ -48,7 +49,7 @@ namespace joint_trajectory_action
 {
 
 const double JointTrajectoryAction::WATCHD0G_PERIOD_ = 1.0;
-const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01;
+const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.001;
 
 JointTrajectoryAction::JointTrajectoryAction() :
   action_server_(node_, "joint_trajectory_action",
@@ -189,118 +190,110 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e, int group_number)
   trajectory_state_recvd_ = false;
 }
 
-void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
-{
-  gh.setAccepted();
-
-  int group_number;
+bool JointTrajectoryAction::allGroupsFinished() const {
+  typedef std::map<int, bool>::const_iterator iterator;
 
-// TODO(thiagodefreitas): change for getting the id from the group instead of a sequential checking on the map
+  for (iterator i = has_active_goal_map_.begin(); i != has_active_goal_map_.end(); ++i) {
+    if (i->second) return false;
+  }
+  return true;
+}
 
+void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh) {
+  gh.setAccepted();
+  int group_number;
+
   ros::Duration last_time_from_start(0.0);
-
+  
   motoman_msgs::DynamicJointTrajectory dyn_traj;
-
-  for (int i = 0; i < gh.getGoal()->trajectory.points.size(); i++)
-  {
+
+  for (int group_index = 0; group_index < robot_groups_.size(); group_index++) {
+    // Tell the rest of the system this robot id has an active goal.
+    has_active_goal_map_[group_index]  = true;
+    // Set the goal as the active goal for this robot id.
+    active_goal_map_[group_index] = gh;
+    current_traj_map_[group_index] = active_goal_map_[group_index].getGoal()->trajectory;
+  }
+
+  for (int i = 0; i < gh.getGoal()->trajectory.points.size(); i++) {
     motoman_msgs::DynamicJointPoint dpoint;
-
-    for (int rbt_idx = 0; rbt_idx < robot_groups_.size(); rbt_idx++)
-    {
-      size_t ros_idx = std::find(
-                         gh.getGoal()->trajectory.joint_names.begin(),
-                         gh.getGoal()->trajectory.joint_names.end(),
-                         robot_groups_[rbt_idx].get_joint_names()[0])
-                       - gh.getGoal()->trajectory.joint_names.begin();
-
-      bool is_found = ros_idx < gh.getGoal()->trajectory.joint_names.size();
-
-      group_number = rbt_idx;
+
+    for (int rbt_idx = 0; rbt_idx < robot_groups_.size(); rbt_idx++) {
+      int num_joints = robot_groups_[rbt_idx].get_joint_names().size();
       motoman_msgs::DynamicJointsGroup dyn_group;
-
-      int num_joints = robot_groups_[group_number].get_joint_names().size();
-
-      if (is_found)
-      {
-        if (gh.getGoal()->trajectory.points[i].positions.empty())
-        {
-          std::vector<double> positions(num_joints, 0.0);
-          dyn_group.positions = positions;
-        }
-        else
+
+      // Check if positions vector is empty.
+      if (gh.getGoal()->trajectory.points[i].positions.empty()) {
+        std::vector<double> positions(num_joints, 0.0);
+        dyn_group.positions = positions;
+      }
+
+      // Check if velocities vector is empty.
+      if (gh.getGoal()->trajectory.points[i].velocities.empty()) {
+        std::vector<double> velocities(num_joints, 0.0);
+        dyn_group.velocities = velocities;
+      }
+
+      // Check if acceleration vector is empty.
+      if (gh.getGoal()->trajectory.points[i].accelerations.empty()) {
+        std::vector<double> accelerations(num_joints, 0.0);
+        dyn_group.accelerations = accelerations;
+      }
+
+      // Check if effort vector is empty.
+      if (gh.getGoal()->trajectory.points[i].effort.empty()) {
+        std::vector<double> effort(num_joints, 0.0);
+        dyn_group.effort = effort;
+      }
+
+      for (int j = 0; j < num_joints; j++) {
+        size_t ros_idx = std::find(gh.getGoal()->trajectory.joint_names.begin(), gh.getGoal()->trajectory.joint_names.end(), robot_groups_[rbt_idx].get_joint_names()[j]) - gh.getGoal()->trajectory.joint_names.begin();
+        group_number = rbt_idx;
+
+        // Insert position value for joint j.
+        if (!gh.getGoal()->trajectory.points[i].positions.empty()) {
           dyn_group.positions.insert(
-            dyn_group.positions.begin(),
-            gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx,
-            gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx
-            + robot_groups_[rbt_idx].get_joint_names().size());
-
-        if (gh.getGoal()->trajectory.points[i].velocities.empty())
-        {
-          std::vector<double> velocities(num_joints, 0.0);
-          dyn_group.velocities = velocities;
+            dyn_group.positions.begin() + j,
+            *(gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx));
         }
-        else
+
+        // Insert velocity value for joint j.
+        if (!gh.getGoal()->trajectory.points[i].velocities.empty()) {
           dyn_group.velocities.insert(
-            dyn_group.velocities.begin(),
-            gh.getGoal()->trajectory.points[i].velocities.begin()
-            + ros_idx, gh.getGoal()->trajectory.points[i].velocities.begin()
-            + ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
-
-        if (gh.getGoal()->trajectory.points[i].accelerations.empty())
-        {
-          std::vector<double> accelerations(num_joints, 0.0);
-          dyn_group.accelerations = accelerations;
+            dyn_group.velocities.begin() + j,
+            *(gh.getGoal()->trajectory.points[i].velocities.begin() + ros_idx));
         }
-        else
+
+        // Insert acceleration value for joint j.
+        if (!gh.getGoal()->trajectory.points[i].accelerations.empty()) {
           dyn_group.accelerations.insert(
-            dyn_group.accelerations.begin(),
-            gh.getGoal()->trajectory.points[i].accelerations.begin()
-            + ros_idx, gh.getGoal()->trajectory.points[i].accelerations.begin()
-            + ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
-        if (gh.getGoal()->trajectory.points[i].effort.empty())
-        {
-          std::vector<double> effort(num_joints, 0.0);
-          dyn_group.effort = effort;
+            dyn_group.accelerations.begin() + j,
+            *(gh.getGoal()->trajectory.points[i].accelerations.begin() + ros_idx));
         }
-        else
+
+        // Insert effort value for joint j.
+        if (!gh.getGoal()->trajectory.points[i].effort.empty()) {
           dyn_group.effort.insert(
-            dyn_group.effort.begin(),
-            gh.getGoal()->trajectory.points[i].effort.begin()
-            + ros_idx, gh.getGoal()->trajectory.points[i].effort.begin()
-            + ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
-        dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
-        dyn_group.group_number = group_number;
-        dyn_group.num_joints = dyn_group.positions.size();
-      }
-
-      // Generating message for groups that were not present in the trajectory message
-      else
-      {
-        std::vector<double> positions(num_joints, 0.0);
-        std::vector<double> velocities(num_joints, 0.0);
-        std::vector<double> accelerations(num_joints, 0.0);
-        std::vector<double> effort(num_joints, 0.0);
-
-        dyn_group.positions = positions;
-        dyn_group.velocities = velocities;
-        dyn_group.accelerations = accelerations;
-        dyn_group.effort = effort;
-
-        dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
-        dyn_group.group_number = group_number;
-        dyn_group.num_joints = num_joints;
+            dyn_group.effort.begin() + j,
+            *(gh.getGoal()->trajectory.points[i].effort.begin() + ros_idx));
+        }
       }
-
+      dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
+      dyn_group.group_number = group_number;
+      dyn_group.num_joints = dyn_group.positions.size();
+
       dpoint.groups.push_back(dyn_group);
     }
     dpoint.num_groups = dpoint.groups.size();
     dyn_traj.points.push_back(dpoint);
   }
+
   dyn_traj.header = gh.getGoal()->trajectory.header;
   dyn_traj.header.stamp = ros::Time::now();
+
   // Publishing the joint names for the 4 groups
   dyn_traj.joint_names = all_joint_names_;
-
+  
   this->pub_trajectory_command_.publish(dyn_traj);
 }
 
@@ -489,15 +482,15 @@ void JointTrajectoryAction::controllerStateCB(
       if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE)
       {
         ROS_INFO("Inside goal constraints, stopped moving, return success for action");
-        active_goal_map_[robot_id].setSucceeded();
         has_active_goal_map_[robot_id] = false;
+        if (allGroupsFinished()) active_goal_map_[robot_id].setSucceeded();
       }
       else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN)
       {
         ROS_INFO("Inside goal constraints, return success for action");
         ROS_WARN("Robot status in motion unknown, the robot driver node and controller code should be updated");
-        active_goal_map_[robot_id].setSucceeded();
         has_active_goal_map_[robot_id] = false;
+        if (allGroupsFinished()) active_goal_map_[robot_id].setSucceeded();
       }
       else
       {
@@ -508,8 +501,8 @@ void JointTrajectoryAction::controllerStateCB(
     {
       ROS_INFO("Inside goal constraints, return success for action");
       ROS_WARN("Robot status is not being published the robot driver node and controller code should be updated");
-      active_goal_map_[robot_id].setSucceeded();
       has_active_goal_map_[robot_id] = false;
+      if (allGroupsFinished()) active_goal_map_[robot_id].setSucceeded();
     }
   }
 }
@@ -674,13 +667,23 @@ bool JointTrajectoryAction::withinGoalConstraints(
   else
   {
     int last_point = traj.points.size() - 1;
-
     int group_number = robot_id;
 
+    std::vector<double> sorted_positions(robot_groups_[group_number].get_joint_names().size());
+    std::vector<std::string> sorted_joint_names(robot_groups_[group_number].get_joint_names().size());
+
+    for (int sort_index = 0; sort_index < robot_groups_[group_number].get_joint_names().size(); sort_index++) {
+        size_t ros_idx = std::find(traj.joint_names.begin(), traj.joint_names.end(), robot_groups_[group_number].get_joint_names()[sort_index]) - traj.joint_names.begin();
+        sorted_positions[sort_index] = traj.points[last_point].positions[ros_idx];
+        sorted_joint_names[sort_index] = traj.joint_names[ros_idx];
+    }
+
     if (industrial_robot_client::utils::isWithinRange(
           robot_groups_[group_number].get_joint_names(),
-          last_trajectory_state_map_[group_number]->actual.positions, traj.joint_names,
-          traj.points[last_point].positions, goal_threshold_))
+          last_trajectory_state_map_[group_number]->actual.positions,
+          sorted_joint_names,
+          sorted_positions,
+          goal_threshold_))
     {
       rtn = true;
     }

motoman_driver/src/simple_message/joint_feedback_ex.cpp

@@ -117,12 +117,23 @@ bool JointFeedbackEx::unload(industrial::byte_array::ByteArray *buffer)
     return false;
   }
 
-  for (int i = 0; i < this->groups_number_; i++)
+  const int num_groups_ignored = MAX_NUM_GROUPS - this->groups_number_;
+
+  for (int i = 0; i < num_groups_ignored; ++i)
   {
-    JointFeedbackMessage tmp_msg;
     JointFeedback j_feedback;
 
+    if (!buffer->unload(j_feedback))
+    {
+      LOG_ERROR("Failed to unload joint feedback groups_number");
+      return false;
+    }
+  }
 
+  for (int i = 0; i < this->groups_number_; i++)
+  {
+    JointFeedbackMessage tmp_msg;
+    JointFeedback j_feedback;
 
     if (!buffer->unload(j_feedback))
     {