diff --git a/launch/gripper_client.launch b/launch/gripper_client.launch index 5468c07573002edde23d9dd034cd7e78177af857..9c84c1511e57191096b4e92a91b94e34829e06f3 100644 --- a/launch/gripper_client.launch +++ b/launch/gripper_client.launch @@ -8,15 +8,9 @@ <remap from="/tiago/gripper_client/gripper_module/gripper_trajectory" to="/gripper_controller/follow_joint_trajectory"/> <!--Services--> - <remap from="/tiago/gripper_client/gripper_module/grasp" + <remap from="~/gripper_module/grasp" to="/gripper_controller/grasp"/> - <param name="~/gripper_module/rate_hz" value="10"/> - <param name="~/gripper_module/action_max_retries" value="5"/> - <param name="~/gripper_module/feedback_watchdog_time_s" value="20"/> - <param name="~/gripper_module/enable_timeout" value="false"/> - <param name="~/gripper_module/timeout_s" value="10"/> - <param name="~/gripper_module/enabled" value="true"/> - + <rosparam file="$(find tiago_modules)/config/gripper_module_default.yaml" command="load" ns="gripper_module" /> </node> <!-- launch dynamic reconfigure --> diff --git a/launch/gripper_client_sim.launch b/launch/gripper_client_sim.launch index fed34d656c9ae56088ceede421026b56a0586b00..ffabd26c55247aff80a97ae401b5873de04a3a9b 100644 --- a/launch/gripper_client_sim.launch +++ b/launch/gripper_client_sim.launch @@ -5,27 +5,6 @@ <arg name="robot" value="steel" /> </include> - <!-- launch the action client node --> - <node name="gripper_client" - pkg="gripper_client" - type="gripper_client" - output="screen" - ns="/tiago"> - <!--Actions--> - <remap from="/tiago/gripper_client/gripper_module/gripper_trajectory" - to="/gripper_controller/follow_joint_trajectory"/> - - <param name="~/gripper_module/rate_hz" value="10"/> - <param name="~/gripper_module/action_max_retries" value="5"/> - <param name="~/gripper_module/feedback_watchdog_time_s" value="20"/> - <param name="~/gripper_module/enable_timeout" value="false"/> - <param name="~/gripper_module/timeout_s" value="10"/> - <param name="~/gripper_module/enabled" value="true"/> - - </node> - - <!-- launch dynamic reconfigure --> - <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false" - output="screen"/> + <include file="$(find gripper_client)/launch/gripper_client.launch"/> </launch> diff --git a/src/gripper_client_alg_node.cpp b/src/gripper_client_alg_node.cpp index d027f7898997a85081aa7c877e539a480148276e..f875c42355cccabe10b8c34c94efa0fe3ea28ade 100644 --- a/src/gripper_client_alg_node.cpp +++ b/src/gripper_client_alg_node.cpp @@ -2,7 +2,7 @@ GripperClientAlgNode::GripperClientAlgNode(void) : algorithm_base::IriBaseAlgorithm<GripperClientAlgorithm>(), - gripper_module("gripper_module") + gripper_module("gripper_module",ros::this_node::getName()) { //init class attributes if necessary this->loop_rate_ = 10;//in [Hz] @@ -66,42 +66,36 @@ void GripperClientAlgNode::node_config_update(Config &config, uint32_t level) //Open grippers if(config.open) { - this->gripper_module.is_finished(); - ROS_INFO("GripperClientAlgNode:Grippers opened successfully!"); + this->gripper_module.open(config.duration); config.open=false; } //Close grippers else if(config.close) { - this->gripper_module.is_finished(); - ROS_INFO("GripperClientAlgNode: Grippers closed successfully!"); + this->gripper_module.close(config.duration); config.close=false; } //Move fingers independently else if(config.move_fingers_to_pos) { this->gripper_module.move_fingers(config.left_finger_pos,config.right_finger_pos,config.duration); - ROS_INFO("GripperClientAlgNode: Fingers moved successfully!"); config.move_fingers_to_pos=false; } //Move fingers to list of positions else if(config.follow_trajectory) { this->gripper_module.move_fingers(this->pos_l,this->pos_r,this->duration); - ROS_INFO("GripperClientAlgNode: Fingers trajectory successfull!"); config.follow_trajectory=false; } else if(config.move_to_distance) { this->gripper_module.fingers_distance(config.fingers_distance,config.duration); - ROS_INFO("GripperClientAlgNode: Fingers moved to distance successfull!"); config.move_to_distance=false; } //Grasp else if(config.grasp) { this->gripper_module.close_grasp(); - ROS_INFO("GripperClientAlgNode: Completed grasping!"); config.grasp=false; } //Stop action