Incorporated usage of controller-id in all relevant parts of the cont…

…roller.

feature_constraints/include/feature_constraints/Controller.h

@@ -49,6 +49,11 @@ class Ranges
 class Controller
 {
 public:
+  // string to identify this controller
+  std::string controller_id_;
+  // string to identify the current movement
+  std::string movement_id_;
+
   // input variables
   std::vector<Constraint> constraints;
   Ranges command, intermediate_command;

feature_constraints/src/Conversions.cpp

@@ -143,6 +143,7 @@ void toMsg(KDL::Jacobian& jac, std::vector<geometry_msgs::Twist>& jac_msg)
 
 void toMsg(Controller& c, constraint_msgs::ConstraintState& msg)
 {
+  msg.controller_id = c.controller_id_;
   toMsg(c.frame, msg.pose);
   toMsg(c.chi, msg.chi);
   toMsg(c.chi_dot, msg.chi_dot);

feature_constraints_standalone/src/constraint_controller.cpp

@@ -139,6 +139,13 @@ bool FeatureConstraintsController::init(ros::NodeHandle &n)
   if(!joint_limit_avoidance_on_)
     ROS_WARN("Joint limit avoidance controller has been deactivated.");
 
+  // read controller-id from parameter server
+  if (!n.getParam("controller_id", feature_controller_.controller_id_)){
+    ROS_ERROR("No controller_id given in namespace: '%s')",
+              n.getNamespace().c_str());
+    return false;
+  }
+
   // resize size of message that reports the state of the joint limit avoidance
   resize(joint_avoidance_state_msg_, dof);
 
@@ -313,7 +320,15 @@ bool FeatureConstraintsController::load_frame_names(ros::NodeHandle& n)
 void FeatureConstraintsController::feature_constraints_callback(const constraint_msgs::ConstraintConfig::ConstPtr& msg)
 {
   boost::mutex::scoped_lock scoped_lock(tf_lookup_mutex_);
-
+
+  // check if we should really process this message
+  if(feature_controller_.controller_id_.compare(msg->controller_id))
+  {
+    ROS_ERROR("Received constraint configuration with non-matching controller-id!: My id: %s, received id: %s Aborting...", 
+      feature_controller_.controller_id_.c_str(), msg->controller_id.c_str());
+    return;
+  }
+
   // get the number of constraints 
   unsigned int num_constraints = msg->constraints.size();
   if(num_constraints == 0)
@@ -417,6 +432,14 @@ void FeatureConstraintsController::feature_constraints_callback(const constraint
  */
 void FeatureConstraintsController::constraint_command_callback(const constraint_msgs::ConstraintCommand::ConstPtr& msg)
 {
+  // check if we should really process this message
+  if(feature_controller_.controller_id_.compare(msg->controller_id))
+  {
+    ROS_ERROR("Received constraint command with non-matching controller-id!: My id: %s, received id: %s Not starting...", 
+      feature_controller_.controller_id_.c_str(), msg->controller_id.c_str());
+    return;
+  }
+
   // TODO(Georg): this sanity check for correspondence of config and command is naive
   //              we already encountered race conditions with this
   if(configured_ && (msg->pos_lo.size() == feature_controller_.command.pos_lo.rows())){

feature_constraints_tests/launch/pr2_pouring_sim.launch

@@ -3,7 +3,7 @@
   <include file="$(find feature_constraints_tests)/launch/pr2_pouring_sim_no_controllers.launch"/>
 
   <!-- load velocity controllers in stopped state -->
-  <include file="$(find ias_mechanism_controllers)/launch/pr2_multi_vel_load.launch"/>
+  <include file="$(find ias_mechanism_controllers)/launch/pr2_multi_vel_started.launch"/>
 
   <!-- LOAD FILTER PARAMETERS FOR CONSTRAINT VELOCITY ESTIMATION -->
   <rosparam command="load" ns="/left_arm_feature_controller" file="$(find feature_constraints_standalone)/launch/constraint_velocity_filter_chain_params.yaml"/>
@@ -14,6 +14,7 @@
     <param name="~robot_base_frame" value="base_link"/>
     <param name="~map_frame" value="base_link"/>
     <param name="~base_frame" value="torso_lift_link"/> <!-- refactor into 'arm_base_frame' -->
+    <param name="~controller_id" value="pr2_left_arm_feature_controller"/>
     <param name="~joint_limit_avoidance" value="false"/>
     <remap from="~qdot" to="/l_arm_vel/command"/>
   </node>