diff --git a/src/tiago_gripper_module.cpp b/src/tiago_gripper_module.cpp index 8dd084889fba2609e4ecc07cb0e3eb70674039aa..d4bb8f5cfe504279cfa387f0b0903cfd18f46a2d 100644 --- a/src/tiago_gripper_module.cpp +++ b/src/tiago_gripper_module.cpp @@ -21,9 +21,7 @@ CTiagoGripperModule::CTiagoGripperModule(const std::string &name,const std::stri else this->is_grasped_watchdog.reset(ros::Duration(this->config.is_grasped_watchdog_time_s)); */ - ROS_INFO("111"); this->is_grasped_subscriber = this->module_nh.subscribe("is_grasped",1,&CTiagoGripperModule::is_grasped_callback,this); - ROS_INFO("222"); /****************Actions******************/ this->goal.trajectory.joint_names.resize(2); @@ -152,9 +150,7 @@ void CTiagoGripperModule::state_machine(void) void CTiagoGripperModule::reconfigure_callback(tiago_gripper_module::TiagoGripperModuleConfig &config, uint32_t level) { - ROS_INFO("CTiagoGripperModule : reconfigure callback"); this->lock(); - ROS_INFO("CTiagoGripperModule : reconfigure callback2"); this->config=config; if(this->goal.trajectory.joint_names.size()>0) { @@ -163,12 +159,10 @@ void CTiagoGripperModule::reconfigure_callback(tiago_gripper_module::TiagoGrippe } /* set the module rate */ this->dynamic_reconfigure(config,"gripper_module"); - ROS_INFO("CTiagoGripperModule : reconfigure callback3"); /* module service parameters */ this->grasp_service.dynamic_reconfigure(config,"grasp"); /* motion action parameters */ this->gripper_action.dynamic_reconfigure(config,"move"); - ROS_INFO("CTiagoGripperModule : reconfigure callback4"); this->unlock(); }