fix #1

moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h

@@ -46,7 +46,7 @@ namespace moveit_simple_controller_manager
 
 
 /*
- * This is an interface for a gripper using control_msgs/GripperCommandAction action interface.
+ * This is an interface for a gripper using control_msgs/GripperCommandAction action interface (single DOF).
  */
 class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs::GripperCommandAction>
 {
@@ -60,36 +60,57 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 
   virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
   {
-    ROS_DEBUG_STREAM("GripperController: new trajectory to " << name_);
+    ROS_DEBUG_STREAM_NAMED("GripperController", "Received new trajectory for " << name_);
 
     if (!controller_action_client_)
       return false;
 
     if (!trajectory.multi_dof_joint_trajectory.points.empty())
     {
-      ROS_ERROR("GripperController: cannot execute multi-dof trajectories.");
+      ROS_ERROR_NAMED("GripperController", "Gripper cannot execute multi-dof trajectories.");
       return false;
     }
 
-    if (trajectory.joint_trajectory.points.size() != 1)
+    if (trajectory.joint_trajectory.points.empty())
     {
-      ROS_ERROR("GripperController: expects a joint trajectory with one point only, but %u provided)", (unsigned int)trajectory.joint_trajectory.points.size());
+      ROS_ERROR_NAMED("GripperController", "GripperController expects a joint trajectory with one point only, but %u provided)", (unsigned int)trajectory.joint_trajectory.points.size());
       return false;
     }
+    if (trajectory.joint_trajectory.points.size() > 1)
+      ROS_WARN_NAMED("GripperController", "GripperController expects a joint trajectory with one point only, but %u provided)", (unsigned int)trajectory.joint_trajectory.points.size());
 
-
-
-    if (trajectory.joint_trajectory.points[0].positions.empty())
+    if (trajectory.joint_trajectory.joint_names.empty())
+    {
+      ROS_ERROR_NAMED("GripperController", "No joint names specified");
+      return false;
+    }
+
+    int gripper_joint_index = -1;
+    for (std::size_t i = 0 ; i < trajectory.joint_trajectory.joint_names.size() ; ++i)
+      if (command_joints_.find(trajectory.joint_trajectory.joint_names[i]) != command_joints_.end())
+      {
+	gripper_joint_index = i;
+	break;
+      }
+    if (gripper_joint_index < 0)
+    {
+      ROS_WARN_NAMED("GripperController", "No command_joint was specified for the MoveIt controller gripper handle. Please see GripperControllerHandle::addCommandJoint() and GripperControllerHandle::setCommandJoint(). Assuming index 0.");
+      gripper_joint_index = 0;
+    }
+
+    if (trajectory.joint_trajectory.points[0].positions.size() <= gripper_joint_index)
     {
-      ROS_ERROR("GripperController: expects a joint trajectory with one point that specifies at least one position, but 0 positions provided)");
+      ROS_ERROR_NAMED("GripperController", "GripperController expects a joint trajectory with one point that specifies at least the position of joint '%s', but insufficient positions provided", 
+                      trajectory.joint_trajectory.joint_names[gripper_joint_index].c_str());
       return false;
     }
 
     /* TODO: currently sending velocity as effort, make this better. */
     control_msgs::GripperCommandGoal goal;
-    if (!trajectory.joint_trajectory.points[0].velocities.empty())
-      goal.command.max_effort = trajectory.joint_trajectory.points[0].velocities[0];
-    goal.command.position = trajectory.joint_trajectory.points[0].positions[0];
+    if (trajectory.joint_trajectory.points[0].velocities.size() > gripper_joint_index)
+      goal.command.max_effort = trajectory.joint_trajectory.points[0].velocities[gripper_joint_index];
+
+    goal.command.position = trajectory.joint_trajectory.points[0].positions[gripper_joint_index];
     controller_action_client_->sendGoal(goal,
                     boost::bind(&GripperControllerHandle::controllerDoneCallback, this, _1, _2),
                     boost::bind(&GripperControllerHandle::controllerActiveCallback, this),
@@ -99,8 +120,21 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     return true;
   }
 
-  void setCommandJoint(const std::string& name) { command_joint_ = name; }
-  void allowFailure(bool allow) { allow_failure_ = allow; }
+  void setCommandJoint(const std::string& name)
+  {
+    command_joints_.clear();
+    addCommandJoint(name);
+  }
+
+  void addCommandJoint(const std::string& name)
+  {
+    command_joints_.insert(name);
+  }
+
+  void allowFailure(bool allow)
+  {
+    allow_failure_ = allow;
+  }
 
 private:
 
@@ -120,7 +154,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 
   void controllerFeedbackCallback(const control_msgs::GripperCommandFeedbackConstPtr& feedback)
   {
-    // TODO?
   }
 
   /*
@@ -130,12 +163,17 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
   bool allow_failure_;
 
   /*
-   * A GripperCommand message has only a single float64 for the "command", thus only a single
-   * joint angle can be sent -- however, due to the complexity of making grippers look correct
-   * in a URDF, they typically have >1 joints. The "command_joint" is the joint whose position
-   * value will be sent in the GripperCommand action.
+   * A GripperCommand message has only a single float64 for the
+   * "command", thus only a single joint angle can be sent -- however,
+   * due to the complexity of making grippers look correct in a URDF,
+   * they typically have >1 joints. The "command_joint" is the joint
+   * whose position value will be sent in the GripperCommand action. A
+   * set of names is provided for acceptable joint names. If any of
+   * the joints specified is found, the value corresponding to that
+   * joint is considered the command. E.g., for a two-arm robot, we
+   * could have two names: one for the joint of each arm.
    */
-  std::string command_joint_;
+  std::set<std::string> command_joints_;
 };