Optional publishing of cartesian_impedance_controller pose

CentroEPiaggio · Jan 19, 2017 · bf6a901 · bf6a901
1 parent 8182d33
commit bf6a901
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 2 deletions.
diff --git a/lwr_controllers/include/lwr_controllers/cartesian_impedance_controller.h b/lwr_controllers/include/lwr_controllers/cartesian_impedance_controller.h
@@ -44,6 +44,7 @@ namespace lwr_controllers
         std::vector<std::string> joint_names_, cart_12_names_, cart_6_names_;
         std::vector<hardware_interface::JointHandle> joint_handles_;
         std::vector<hardware_interface::CartesianVariableHandle> cart_handles_;
+        bool publish_cartesian_pose_;
 
         // ROS API (topic, service and dynamic reconfigure)
 		ros::Subscriber sub_command_;

diff --git a/lwr_controllers/src/cartesian_impedance_controller.cpp b/lwr_controllers/src/cartesian_impedance_controller.cpp
@@ -50,6 +50,10 @@ namespace lwr_controllers
         {
             ROS_WARN_STREAM("CartesianImpedanceController: robot tip name is " << tip_name_ << ". In this controller it is ignored. Using tip name defined in #TOOL in the KRC!!!");
         }
+        if (n.getParam("publish_cartesian_pose", publish_cartesian_pose_) && publish_cartesian_pose_)
+        {
+            ROS_WARN_STREAM("Publishing the cartesian position of the robot tip (the #TOOL defined in the KRC!!!)");
+        }
 
         joint_names_.push_back( robot_namespace_ + std::string("_a1_joint") );
         joint_names_.push_back( robot_namespace_ + std::string("_a2_joint") );
@@ -98,7 +102,10 @@ namespace lwr_controllers
         srv_command_ = n.advertiseService("set_command", &CartesianImpedanceController::command_cb, this);
         sub_ft_measures_ = n.subscribe(n.resolveName("ft_measures"), 1, &CartesianImpedanceController::updateFT, this);
         pub_goal_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("goal"),0);
-        realtime_pose_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(n, "cartesian_pose", 4));
+        if (publish_cartesian_pose_)
+        {
+            realtime_pose_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(n, "cartesian_pose", 4));
+        }
 
         // // may be needed, to set initial commands asap
         // starting(ros::Time::now());
@@ -178,7 +185,10 @@ namespace lwr_controllers
         // std::cout << "Update current values" << std::endl;
         getCurrentPose(x_cur_);
 
-        publishCurrentPose(x_cur_);
+        if (publish_cartesian_pose_)
+        {
+            publishCurrentPose(x_cur_);
+        }
 
         // forward commands to hwi
         // std::cout << "Before forwarding the command" << std::endl;

diff --git a/single_lwr_example/single_lwr_robot/config/controllers.yaml b/single_lwr_example/single_lwr_robot/config/controllers.yaml
@@ -71,6 +71,7 @@ lwr:
     robot_name: lwr
     root_name: lwr_base_link
     tip_name: lwr_7_link
+    publish_cartesian_pose: false
 
   # Multi Task Priority Inverse Kinematics
   multi_task_priority_inverse_kinematics: