diff --git a/cfg/IriJointsClient.cfg b/cfg/IriJointsClient.cfg index c2ad06bbb2b8907519ffa05adff077dec11ec281..f409dfaba6ad18ea8ab8428dd3b367618a4f37ad 100755 --- a/cfg/IriJointsClient.cfg +++ b/cfg/IriJointsClient.cfg @@ -51,9 +51,9 @@ point_to.add("send_point_to", bool_t, 0, "Ca point_to.add("cancel_point_to", bool_t, 0, "Cancel point_to goal", False) point_to.add("ik_tol", double_t, 0, "Inverse kinematics solver tolerance", 0.001, 0.0, 1.0) point_to.add("send_ik_tol", bool_t, 0, "Send inverse kinematic tolerance", False) -point_to.add("base_frame", str_t, 0, "Kinematic chain base frame", "servo2") +point_to.add("base_frame", str_t, 0, "Kinematic chain base frame", "pan_frame") point_to.add("send_base_frame", bool_t, 0, "Send Kinematic chain base frame", False) -point_to.add("urdf_param", str_t, 0, "ROS param where the urdf model is stored", "robot_description") +point_to.add("urdf_param", str_t, 0, "ROS param where the urdf model is stored", "/robot_description") point_to.add("send_urdf_param", bool_t, 0, "Send the ROS param where the urdf model is stored", False) tracker.add("start_tracking", bool_t, 0, "Start tracking action", False) diff --git a/cfg/IriJointsModule.cfg b/cfg/IriJointsModule.cfg index bd1bebbfa4e7665a744df39cca9be442ee41dd5a..f21e0eb95853fd29a03fb81f4ddd802b4f9297d8 100755 --- a/cfg/IriJointsModule.cfg +++ b/cfg/IriJointsModule.cfg @@ -49,7 +49,7 @@ axis_enum = gen.enum([ gen.const("x", int_t, 0, "Pointing axis x"), # Name Type Reconfiguration level Description Default Min Max #gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0) -gen.add("rate_hz", double_t, 0, "Navigation module rate in Hz", 1.0, 1.0, 100.0) +gen.add("rate_hz", double_t, 0, "Joints module rate in Hz", 1.0, 1.0, 100.0) gen.add("frame_id", str_t, 0, "Joints module frame id", "joints_module") joint_states.add("joint_states_watchdog_time_s", double_t, 0, "Maximum time between JointStates messages",1, 0.01, 50) @@ -70,7 +70,11 @@ tracker.add("tracker_feedback_watchdog_time_s", double_t, 0, tracker.add("tracker_enable_timeout", bool_t, 0, "Enable action timeout", True) tracker.add("tracker_timeout_s", double_t, 0, "Maximum time allowed to complete the action", 5, 0.1, 6000) -move_to_angles.add("move_to_angles_time_from_start", int_t, 0, "Max time in seconds to finish the action", 10, 0, 100) +move_to_angles.add("move_to_angles_time_from_start", int_t, 0, "Max time in seconds to finish the action", 10, 0, 100) +move_to_angles.add("move_to_angles_pos_tol", double_t, 0, "Path and goal position tolerance in rad or m", 0.002, -1) +move_to_angles.add("move_to_angles_vel_tol", double_t, 0, "Path and goal velocity tolerance in rad/s or m/s", 0.0, -1) +move_to_angles.add("move_to_angles_acc_tol", double_t, 0, "Path and goal acceleration tolerance in rad/s2 or m/s2", 0.0, -1) +move_to_angles.add("move_to_angles_time_tol", double_t, 0, "Path and goal time tolerance in percentaje", 0.1, 0.0, 1.0) move_to_angles.add("move_to_angles_max_retries", int_t, 0, "Maximum number of retries for the action start", 1, 1, 10) move_to_angles.add("move_to_angles_enable_watchdog", bool_t, 0, "Enable action watchdog", True) diff --git a/config/joints_module_default.yaml b/config/joints_module_default.yaml index 8d762fdb6dc6a98d45788a4299fb8102c1c9d94a..c4d12368b591727c124357c18be46b9a54a61885 100644 --- a/config/joints_module_default.yaml +++ b/config/joints_module_default.yaml @@ -3,7 +3,7 @@ frame_id: joints_module joint_states_watchdog_time_s: 1.0 -pointing_frame: "load_pointing" +pointing_frame: "camera_optical_frame" pointing_axis: 2 point_frame: "world" @@ -19,7 +19,11 @@ tracker_feedback_watchdog_time_s: 1.0 tracker_enable_timeout: True tracker_timeout_s: 10 -move_to_angles_time_from_start: 10 +move_to_angles_time_from_start: 0 +move_to_angles_pos_tol: 0.02 +move_to_angles_vel_tol: -1.0 +move_to_angles_acc_tol: -1.0 +move_to_angles_time_tol: 0.0 move_to_angles_max_retries: 1 move_to_angles_enable_watchdog: True diff --git a/launch/joints_client.launch b/launch/joints_client.launch index 6eaadfcc652314df372055f030f51aa46f2e4948..24b5773bc8fc5d047be9a00241b9336fbb1d0135 100644 --- a/launch/joints_client.launch +++ b/launch/joints_client.launch @@ -2,13 +2,13 @@ <launch> - <arg name="config_file" default="$(find iri_joints_module)/config/joint_module_ccyt_demo_config.yaml" /> + <arg name="config_file" default="$(find iri_joints_module)/config/joints_module_default.yaml" /> <arg name="node_name" default="joints_module"/> <arg name="output" default="screen"/> <arg name="launch_prefix" default=""/> <arg name="point_to_node_name" default="point_to"/> - <arg name="device_ns" default="ana"/> - <arg name="ns" default="ana"/> + <arg name="device_ns" default="two_servos"/> + <arg name="ns" default="two_servos"/> <node name="$(arg node_name)" pkg="iri_joints_module" @@ -26,7 +26,7 @@ <remap from="~/$(arg node_name)/tracker_point_to" to="/$(arg ns)/$(arg point_to_node_name)/tracker_point_to"/> <remap from="~/$(arg node_name)/move_to_angle" - to="/$(arg device_ns)/effort_controller/follow_joint_trajectory"/> + to="/$(arg device_ns)/joint_trajectory_gazebo/joint_motion"/> <rosparam file="$(arg config_file)" command="load" ns="joints_module" /> </node> diff --git a/launch/joints_client_bt.launch b/launch/joints_client_bt.launch index 3e94189187709f1620dbdd317c4af7366cb630c7..66c5dd5f4e8383460ad8c56bb1bac49084388363 100644 --- a/launch/joints_client_bt.launch +++ b/launch/joints_client_bt.launch @@ -2,16 +2,16 @@ <launch> - <arg name="config_file" default="$(find iri_joints_module)/config/joint_module_ccyt_demo_config.yaml" /> + <arg name="config_file" default="$(find iri_joints_module)/config/joints_module_default.yaml" /> <arg name="node_name" default="joints_module"/> <arg name="output" default="screen"/> <arg name="launch_prefix" default=""/> <arg name="point_to_node_name" default="point_to"/> - <arg name="device_ns" default="ana"/> - <arg name="ns" default="ana"/> + <arg name="device_ns" default="two_servos"/> + <arg name="ns" default="two_servos"/> <arg name="xml_path" default="$(find iri_joints_module)/src/xml"/> - <arg name="xml_file" default="bt_point_tracker_test.xml"/> + <arg name="xml_file" default="bt_move_to_angles_test.xml"/> <arg name="send_ticks" default="False"/> <node name="$(arg node_name)" @@ -30,7 +30,7 @@ <remap from="~/$(arg node_name)/tracker_point_to" to="/$(arg ns)/$(arg point_to_node_name)/tracker_point_to"/> <remap from="~/$(arg node_name)/move_to_angle" - to="/$(arg device_ns)/effort_controller/follow_joint_trajectory"/> + to="/$(arg device_ns)/joint_trajectory_gazebo/joint_motion"/> <param name="xml_path" value="$(arg xml_path)"/> <param name="xml_file" value="$(arg xml_file)"/> <param name="send_ticks" value="$(arg send_ticks)"/> @@ -38,8 +38,8 @@ </node> <!-- launch dynamic reconfigure --> -<!-- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false" - output="screen"/>--> + <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false" + output="screen"/> </launch> diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp index eeb5484361a9290e54fc91784a46727d07432326..684285cebddf65827fc51e396612a27f7b50d698 100644 --- a/src/iri_joints_module.cpp +++ b/src/iri_joints_module.cpp @@ -536,8 +536,22 @@ bool CIriJointsModule::move_to_angles(std::vector<std::string> joint_names, std: std::vector<std::string>().swap(this->move_to_angle_goal.trajectory.joint_names); this->move_to_angle_goal.trajectory.joint_names = joint_names; + control_msgs::JointTolerance tol; + tol.position = this->config.move_to_angles_pos_tol; + tol.velocity = this->config.move_to_angles_vel_tol; + tol.acceleration = this->config.move_to_angles_acc_tol; + std::vector<control_msgs::JointTolerance>().swap(this->move_to_angle_goal.path_tolerance); + std::vector<control_msgs::JointTolerance>().swap(this->move_to_angle_goal.goal_tolerance); + for (uint8_t i; i < this->move_to_angle_goal.trajectory.joint_names.size(); i++) + { + tol.name = this->move_to_angle_goal.trajectory.joint_names[i]; + this->move_to_angle_goal.path_tolerance.push_back(tol); + this->move_to_angle_goal.goal_tolerance.push_back(tol); + } + trajectory_msgs::JointTrajectoryPoint point; point.time_from_start = ros::Duration(this->config.move_to_angles_time_from_start); + this->move_to_angle_goal.goal_time_tolerance = point.time_from_start*this->config.move_to_angles_time_tol; // ROS_INFO_STREAM("Max time " << point.time_from_start); for (uint8_t i = 0; i < angles.size(); i++) {