diff --git a/CMakeLists.txt b/CMakeLists.txt index 581e61db..66dd650a 120000 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml index 05e23931..5c2f22bf 100644 --- a/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml @@ -1,17 +1,17 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_joint_trajectory_controller: +$(arg prefix)gen3_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -19,55 +19,55 @@ gen3_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml index bb6fa834..621a0b30 100644 --- a/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml @@ -1,18 +1,18 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_joint_trajectory_controller: +$(arg prefix)gen3_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -20,64 +20,64 @@ gen3_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 3000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_7_position_controller: - joint: joint_7 +$(arg prefix)joint_7_position_controller: + joint: $(arg prefix)joint_7 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml index 05c1c470..c869f63a 100644 --- a/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml @@ -1,17 +1,17 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_lite_joint_trajectory_controller: +$(arg prefix)gen3_lite_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -19,55 +19,55 @@ gen3_lite_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml index 7d8c81d0..87ac60a0 100644 --- a/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml @@ -1,11 +1,11 @@ -gen3_lite_2f_gripper_controller: +$(arg prefix)gen3_lite_2f_gripper_controller: type: position_controllers/GripperActionController - joint: right_finger_bottom_joint + joint: $(arg prefix)right_finger_bottom_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0} - right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} - left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} - left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file + $(arg prefix)right_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} + $(arg prefix)right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} + $(arg prefix)left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} + $(arg prefix)left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file diff --git a/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml index bc61292d..869f515a 100644 --- a/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml @@ -1,13 +1,13 @@ -robotiq_2f_140_gripper_controller: +$(arg prefix)robotiq_2f_140_gripper_controller: type: position_controllers/GripperActionController - joint: finger_joint + joint: $(arg prefix)finger_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - finger_joint: {p: 10.0, i: 0.0, d: 0.01} - right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} - right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} - right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file + $(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} + $(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} + $(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file diff --git a/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml index dc06e4f8..4e40277e 100644 --- a/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml @@ -1,13 +1,13 @@ -robotiq_2f_85_gripper_controller: +$(arg prefix)robotiq_2f_85_gripper_controller: type: position_controllers/GripperActionController - joint: finger_joint + joint: $(arg prefix)finger_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - finger_joint: {p: 10.0, i: 0.0, d: 0.01} - right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} - right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} - right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file + $(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} + $(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} + $(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file diff --git a/kortex_control/package.xml b/kortex_control/package.xml index 935cfab5..f937c188 100644 --- a/kortex_control/package.xml +++ b/kortex_control/package.xml @@ -1,12 +1,12 @@ kortex_control - 2.2.0 + 2.2.2 -

Gazebo Ros Control package for Kortex robots

-

This package contains launch and configuration files for the ros_control controllers for simulation

+

Gazebo ROS Control package for simulated Kortex robots

+

This package contains configuration files for the gazebo_ros_control controllers for simulation of Kortex arms

 Alexandre Vannobel - + BSD catkin roslaunch diff --git a/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml b/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml index 62f19ff8..729f82b9 100644 --- a/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml +++ b/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml @@ -1,6 +1,6 @@ -initial_joint_positions: "-J joint_1 1.57 - -J joint_2 -0.35 - -J joint_3 -2.00 - -J joint_4 0 - -J joint_5 -1.00 - -J joint_6 1.57 " \ No newline at end of file +initial_joint_positions: "-J $(arg prefix)joint_1 1.57 + -J $(arg prefix)joint_2 -0.35 + -J $(arg prefix)joint_3 -2.00 + -J $(arg prefix)joint_4 0 + -J $(arg prefix)joint_5 -1.00 + -J $(arg prefix)joint_6 1.57 " \ No newline at end of file diff --git a/kortex_description/arms/gen3/6dof/config/joint_limits.yaml b/kortex_description/arms/gen3/6dof/config/joint_limits.yaml index 8692c090..e524d93f 100644 --- a/kortex_description/arms/gen3/6dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3/6dof/config/joint_limits.yaml @@ -1,7 +1,21 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 +maximum_velocities: + - 0.8727 + - 0.8727 + - 0.8727 + - 0.8727 + - 0.8727 + - 0.8727 +maximum_accelerations: + - 1.0 + - 1.0 + - 1.0 + - 10.0 + - 10.0 + - 10.0 diff --git a/kortex_description/arms/gen3/6dof/config/twist_limits.yaml b/kortex_description/arms/gen3/6dof/config/twist_limits.yaml new file mode 100644 index 00000000..7b71f618 --- /dev/null +++ b/kortex_description/arms/gen3/6dof/config/twist_limits.yaml @@ -0,0 +1,4 @@ +maximum_linear_velocity: 0.5 +maximum_angular_velocity: 1.74 +maximum_linear_acceleration: 0.4 +maximum_angular_acceleration: 0.4 \ No newline at end of file diff --git a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro index db152ab5..c12d737b 100644 --- a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro +++ b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro @@ -228,11 +228,11 @@ - + - + - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,24 +16,24 @@ - right_finger_bottom_joint - right_finger_tip_joint + ${prefix}right_finger_bottom_joint + ${prefix}right_finger_tip_joint -0.676 0.149 5.0 - right_finger_bottom_joint - left_finger_bottom_joint + ${prefix}right_finger_bottom_joint + ${prefix}left_finger_bottom_joint -1.0 0.0 5.0 - right_finger_bottom_joint - left_finger_tip_joint + ${prefix}right_finger_bottom_joint + ${prefix}left_finger_tip_joint -0.676 0.149 5.0 @@ -43,9 +43,9 @@ my_gen3_lite_gripper - end_effector_link - right_finger_dist_link - left_finger_dist_link + ${prefix}end_effector_link + ${prefix}right_finger_dist_link + ${prefix}left_finger_dist_link 100 10 diff --git a/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml b/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml index 3c16e30a..dd1fd9e8 100644 --- a/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml +++ b/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - finger_joint + - $(arg prefix)finger_joint gripper_joint_limits_min: - 0.000 diff --git a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro index 11adb98f..f1a85b8c 100644 --- a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro +++ b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,40 +16,40 @@ - finger_joint - right_outer_knuckle_joint + ${prefix}finger_joint + ${prefix}right_outer_knuckle_joint -1.0 0.0 5.0 - finger_joint - right_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}right_inner_knuckle_joint -1.0 0.0 5.0 - finger_joint - left_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}left_inner_knuckle_joint -1.0 0.0 5.0 - finger_joint - left_inner_finger_joint + ${prefix}finger_joint + ${prefix}left_inner_finger_joint 1.0 0.0 5.0 - finger_joint - right_inner_finger_joint + ${prefix}finger_joint + ${prefix}right_inner_finger_joint 1.0 0.0 5.0 @@ -59,9 +59,9 @@ my_robotiq_gripper - bracelet_link - left_inner_finger - right_inner_finger + ${prefix}bracelet_link + ${prefix}left_inner_finger + ${prefix}right_inner_finger 100 10 diff --git a/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml b/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml index 3c64028a..d67e31fe 100644 --- a/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml +++ b/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - finger_joint + - $(arg prefix)finger_joint gripper_joint_limits_min: - 0.000 diff --git a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro index fe287f37..4ed28bab 100644 --- a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro +++ b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,40 +16,40 @@ - finger_joint - right_outer_knuckle_joint + ${prefix}finger_joint + ${prefix}right_outer_knuckle_joint 1.0 0.0 5.0 - finger_joint - right_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}right_inner_knuckle_joint 1.0 0.0 5.0 - finger_joint - left_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}left_inner_knuckle_joint 1.0 0.0 5.0 - finger_joint - left_inner_finger_joint + ${prefix}finger_joint + ${prefix}left_inner_finger_joint -1.0 0.0 5.0 - finger_joint - right_inner_finger_joint + ${prefix}finger_joint + ${prefix}right_inner_finger_joint -1.0 0.0 5.0 @@ -59,9 +59,9 @@ my_robotiq_gripper - bracelet_link - left_inner_finger - right_inner_finger + ${prefix}bracelet_link + ${prefix}left_inner_finger + ${prefix}right_inner_finger 100 10 diff --git a/kortex_description/launch/visualize.launch b/kortex_description/launch/visualize.launch index 30914192..6fe5e26f 100644 --- a/kortex_description/launch/visualize.launch +++ b/kortex_description/launch/visualize.launch @@ -14,10 +14,10 @@ - - diff --git a/kortex_description/package.xml b/kortex_description/package.xml index 5f0ba38b..018c38c1 100644 --- a/kortex_description/package.xml +++ b/kortex_description/package.xml @@ -1,12 +1,13 @@ kortex_description - 2.2.1 + 2.2.2 -

URDF Description package for Kortex-compatible robots

+

URDF and xacro description package for Kortex robots

This package contains configuration data, 3D models and launch files -for Kortex-compatible arms and grippers

+for Kortex arms and supported grippers

 - + Alexandre Vannobel + BSD catkin roslaunch diff --git a/kortex_description/readme.md b/kortex_description/readme.md index 2ecf830b..1811c64e 100644 --- a/kortex_description/readme.md +++ b/kortex_description/readme.md @@ -23,10 +23,10 @@ To load the description of a robot, you simply have to load the **ARM.xacro** or For example: - To load the Gen3 description with a Robotiq 2-F 85 gripper for simulation, you would put in your launch file : - + - To load the Gen3 lite description, you would put in your launch file : - + ## Tool frame diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index c092cfdd..7e2c4752 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -17,10 +17,10 @@ if(NOT CMAKE_BUILD_TYPE) endif() ## find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs controller_manager_msgs message_generation actionlib control_msgs urdf moveit_ros_planning_interface) find_package(Boost REQUIRED COMPONENTS system) -file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/*.cpp") +file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/robot/*.cpp" "src/generated/simulation/*.cpp") file(GLOB_RECURSE non_generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/non-generated/driver/*.cpp") file(GLOB_RECURSE test_files RELATIVE ${PROJECT_SOURCE_DIR} "src/non-generated/tests/*.cc") diff --git a/kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h deleted file mode 100644 index 7ec46094..00000000 --- a/kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h +++ /dev/null @@ -1,119 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_ACTUATORCONFIG_SERVICES_H_ -#define _KORTEX_ACTUATORCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/GetAxisOffsets.h" -#include "kortex_driver/SetAxisOffsets.h" -#include "kortex_driver/SetTorqueOffset.h" -#include "kortex_driver/ActuatorConfig_GetControlMode.h" -#include "kortex_driver/SetControlMode.h" -#include "kortex_driver/GetActivatedControlLoop.h" -#include "kortex_driver/SetActivatedControlLoop.h" -#include "kortex_driver/GetControlLoopParameters.h" -#include "kortex_driver/SetControlLoopParameters.h" -#include "kortex_driver/SelectCustomData.h" -#include "kortex_driver/GetSelectedCustomData.h" -#include "kortex_driver/SetCommandMode.h" -#include "kortex_driver/ActuatorConfig_ClearFaults.h" -#include "kortex_driver/SetServoing.h" -#include "kortex_driver/MoveToPosition.h" -#include "kortex_driver/GetCommandMode.h" -#include "kortex_driver/GetServoing.h" -#include "kortex_driver/GetTorqueOffset.h" -#include "kortex_driver/SetCoggingFeedforwardMode.h" -#include "kortex_driver/GetCoggingFeedforwardMode.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class ActuatorConfigServices -{ - public: - ActuatorConfigServices(ros::NodeHandle& n, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res); - bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res); - bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res); - bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res); - bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res); - bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res); - bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res); - bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res); - bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res); - bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res); - bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res); - bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res); - bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res); - bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res); - bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res); - bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res); - bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res); - bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res); - bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res); - bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::ActuatorConfig::ActuatorConfigClient* m_actuatorconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceGetAxisOffsets; - ros::ServiceServer m_serviceSetAxisOffsets; - ros::ServiceServer m_serviceSetTorqueOffset; - ros::ServiceServer m_serviceActuatorConfig_GetControlMode; - ros::ServiceServer m_serviceSetControlMode; - ros::ServiceServer m_serviceGetActivatedControlLoop; - ros::ServiceServer m_serviceSetActivatedControlLoop; - ros::ServiceServer m_serviceGetControlLoopParameters; - ros::ServiceServer m_serviceSetControlLoopParameters; - ros::ServiceServer m_serviceSelectCustomData; - ros::ServiceServer m_serviceGetSelectedCustomData; - ros::ServiceServer m_serviceSetCommandMode; - ros::ServiceServer m_serviceActuatorConfig_ClearFaults; - ros::ServiceServer m_serviceSetServoing; - ros::ServiceServer m_serviceMoveToPosition; - ros::ServiceServer m_serviceGetCommandMode; - ros::ServiceServer m_serviceGetServoing; - ros::ServiceServer m_serviceGetTorqueOffset; - ros::ServiceServer m_serviceSetCoggingFeedforwardMode; - ros::ServiceServer m_serviceGetCoggingFeedforwardMode; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/base_services.h b/kortex_driver/include/kortex_driver/generated/base_services.h deleted file mode 100644 index ecfa5253..00000000 --- a/kortex_driver/include/kortex_driver/generated/base_services.h +++ /dev/null @@ -1,547 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_BASE_SERVICES_H_ -#define _KORTEX_BASE_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/CreateUserProfile.h" -#include "kortex_driver/UpdateUserProfile.h" -#include "kortex_driver/ReadUserProfile.h" -#include "kortex_driver/DeleteUserProfile.h" -#include "kortex_driver/ReadAllUserProfiles.h" -#include "kortex_driver/ReadAllUsers.h" -#include "kortex_driver/ChangePassword.h" -#include "kortex_driver/CreateSequence.h" -#include "kortex_driver/UpdateSequence.h" -#include "kortex_driver/ReadSequence.h" -#include "kortex_driver/DeleteSequence.h" -#include "kortex_driver/ReadAllSequences.h" -#include "kortex_driver/PlaySequence.h" -#include "kortex_driver/PlayAdvancedSequence.h" -#include "kortex_driver/StopSequence.h" -#include "kortex_driver/PauseSequence.h" -#include "kortex_driver/ResumeSequence.h" -#include "kortex_driver/CreateProtectionZone.h" -#include "kortex_driver/UpdateProtectionZone.h" -#include "kortex_driver/ReadProtectionZone.h" -#include "kortex_driver/DeleteProtectionZone.h" -#include "kortex_driver/ReadAllProtectionZones.h" -#include "kortex_driver/CreateMapping.h" -#include "kortex_driver/ReadMapping.h" -#include "kortex_driver/UpdateMapping.h" -#include "kortex_driver/DeleteMapping.h" -#include "kortex_driver/ReadAllMappings.h" -#include "kortex_driver/CreateMap.h" -#include "kortex_driver/ReadMap.h" -#include "kortex_driver/UpdateMap.h" -#include "kortex_driver/DeleteMap.h" -#include "kortex_driver/ReadAllMaps.h" -#include "kortex_driver/ActivateMap.h" -#include "kortex_driver/CreateAction.h" -#include "kortex_driver/ReadAction.h" -#include "kortex_driver/ReadAllActions.h" -#include "kortex_driver/DeleteAction.h" -#include "kortex_driver/UpdateAction.h" -#include "kortex_driver/ExecuteActionFromReference.h" -#include "kortex_driver/ExecuteAction.h" -#include "kortex_driver/PauseAction.h" -#include "kortex_driver/StopAction.h" -#include "kortex_driver/ResumeAction.h" -#include "kortex_driver/GetIPv4Configuration.h" -#include "kortex_driver/SetIPv4Configuration.h" -#include "kortex_driver/SetCommunicationInterfaceEnable.h" -#include "kortex_driver/IsCommunicationInterfaceEnable.h" -#include "kortex_driver/GetAvailableWifi.h" -#include "kortex_driver/GetWifiInformation.h" -#include "kortex_driver/AddWifiConfiguration.h" -#include "kortex_driver/DeleteWifiConfiguration.h" -#include "kortex_driver/GetAllConfiguredWifis.h" -#include "kortex_driver/ConnectWifi.h" -#include "kortex_driver/DisconnectWifi.h" -#include "kortex_driver/GetConnectedWifiInformation.h" -#include "kortex_driver/Base_Unsubscribe.h" -#include "kortex_driver/OnNotificationConfigurationChangeTopic.h" -#include "kortex_driver/ConfigurationChangeNotification.h" -#include "kortex_driver/OnNotificationMappingInfoTopic.h" -#include "kortex_driver/MappingInfoNotification.h" -#include "kortex_driver/OnNotificationControlModeTopic.h" -#include "kortex_driver/ControlModeNotification.h" -#include "kortex_driver/OnNotificationOperatingModeTopic.h" -#include "kortex_driver/OperatingModeNotification.h" -#include "kortex_driver/OnNotificationSequenceInfoTopic.h" -#include "kortex_driver/SequenceInfoNotification.h" -#include "kortex_driver/OnNotificationProtectionZoneTopic.h" -#include "kortex_driver/ProtectionZoneNotification.h" -#include "kortex_driver/OnNotificationUserTopic.h" -#include "kortex_driver/UserNotification.h" -#include "kortex_driver/OnNotificationControllerTopic.h" -#include "kortex_driver/ControllerNotification.h" -#include "kortex_driver/OnNotificationActionTopic.h" -#include "kortex_driver/ActionNotification.h" -#include "kortex_driver/OnNotificationRobotEventTopic.h" -#include "kortex_driver/RobotEventNotification.h" -#include "kortex_driver/PlayCartesianTrajectory.h" -#include "kortex_driver/PlayCartesianTrajectoryPosition.h" -#include "kortex_driver/PlayCartesianTrajectoryOrientation.h" -#include "kortex_driver/Stop.h" -#include "kortex_driver/GetMeasuredCartesianPose.h" -#include "kortex_driver/SendWrenchCommand.h" -#include "kortex_driver/SendWrenchJoystickCommand.h" -#include "kortex_driver/SendTwistJoystickCommand.h" -#include "kortex_driver/SendTwistCommand.h" -#include "kortex_driver/PlayJointTrajectory.h" -#include "kortex_driver/PlaySelectedJointTrajectory.h" -#include "kortex_driver/GetMeasuredJointAngles.h" -#include "kortex_driver/SendJointSpeedsCommand.h" -#include "kortex_driver/SendSelectedJointSpeedCommand.h" -#include "kortex_driver/SendGripperCommand.h" -#include "kortex_driver/GetMeasuredGripperMovement.h" -#include "kortex_driver/SetAdmittance.h" -#include "kortex_driver/SetOperatingMode.h" -#include "kortex_driver/ApplyEmergencyStop.h" -#include "kortex_driver/Base_ClearFaults.h" -#include "kortex_driver/Base_GetControlMode.h" -#include "kortex_driver/GetOperatingMode.h" -#include "kortex_driver/SetServoingMode.h" -#include "kortex_driver/GetServoingMode.h" -#include "kortex_driver/OnNotificationServoingModeTopic.h" -#include "kortex_driver/ServoingModeNotification.h" -#include "kortex_driver/RestoreFactorySettings.h" -#include "kortex_driver/OnNotificationFactoryTopic.h" -#include "kortex_driver/FactoryNotification.h" -#include "kortex_driver/GetAllConnectedControllers.h" -#include "kortex_driver/GetControllerState.h" -#include "kortex_driver/GetActuatorCount.h" -#include "kortex_driver/StartWifiScan.h" -#include "kortex_driver/GetConfiguredWifi.h" -#include "kortex_driver/OnNotificationNetworkTopic.h" -#include "kortex_driver/NetworkNotification.h" -#include "kortex_driver/GetArmState.h" -#include "kortex_driver/OnNotificationArmStateTopic.h" -#include "kortex_driver/ArmStateNotification.h" -#include "kortex_driver/GetIPv4Information.h" -#include "kortex_driver/SetWifiCountryCode.h" -#include "kortex_driver/GetWifiCountryCode.h" -#include "kortex_driver/Base_SetCapSenseConfig.h" -#include "kortex_driver/Base_GetCapSenseConfig.h" -#include "kortex_driver/GetAllJointsSpeedHardLimitation.h" -#include "kortex_driver/GetAllJointsTorqueHardLimitation.h" -#include "kortex_driver/GetTwistHardLimitation.h" -#include "kortex_driver/GetWrenchHardLimitation.h" -#include "kortex_driver/SendJointSpeedsJoystickCommand.h" -#include "kortex_driver/SendSelectedJointSpeedJoystickCommand.h" -#include "kortex_driver/EnableBridge.h" -#include "kortex_driver/DisableBridge.h" -#include "kortex_driver/GetBridgeList.h" -#include "kortex_driver/GetBridgeConfig.h" -#include "kortex_driver/PlayPreComputedJointTrajectory.h" -#include "kortex_driver/GetProductConfiguration.h" -#include "kortex_driver/UpdateEndEffectorTypeConfiguration.h" -#include "kortex_driver/RestoreFactoryProductConfiguration.h" -#include "kortex_driver/GetTrajectoryErrorReport.h" -#include "kortex_driver/GetAllJointsSpeedSoftLimitation.h" -#include "kortex_driver/GetAllJointsTorqueSoftLimitation.h" -#include "kortex_driver/GetTwistSoftLimitation.h" -#include "kortex_driver/GetWrenchSoftLimitation.h" -#include "kortex_driver/SetControllerConfigurationMode.h" -#include "kortex_driver/GetControllerConfigurationMode.h" -#include "kortex_driver/StartTeaching.h" -#include "kortex_driver/StopTeaching.h" -#include "kortex_driver/AddSequenceTasks.h" -#include "kortex_driver/UpdateSequenceTask.h" -#include "kortex_driver/SwapSequenceTasks.h" -#include "kortex_driver/ReadSequenceTask.h" -#include "kortex_driver/ReadAllSequenceTasks.h" -#include "kortex_driver/DeleteSequenceTask.h" -#include "kortex_driver/DeleteAllSequenceTasks.h" -#include "kortex_driver/TakeSnapshot.h" -#include "kortex_driver/GetFirmwareBundleVersions.h" -#include "kortex_driver/MoveSequenceTask.h" -#include "kortex_driver/DuplicateMapping.h" -#include "kortex_driver/DuplicateMap.h" -#include "kortex_driver/SetControllerConfiguration.h" -#include "kortex_driver/GetControllerConfiguration.h" -#include "kortex_driver/GetAllControllerConfigurations.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class BaseServices -{ - public: - BaseServices(ros::NodeHandle& n, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res); - bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res); - bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res); - bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res); - bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res); - bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res); - bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res); - bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res); - bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res); - bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res); - bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res); - bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res); - bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res); - bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res); - bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res); - bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res); - bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res); - bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res); - bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res); - bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res); - bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res); - bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res); - bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res); - bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res); - bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res); - bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res); - bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res); - bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res); - bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res); - bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res); - bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res); - bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res); - bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res); - bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res); - bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res); - bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res); - bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res); - bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res); - bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res); - bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res); - bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res); - bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res); - bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res); - bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res); - bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res); - bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res); - bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res); - bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res); - bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res); - bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res); - bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res); - bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res); - bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res); - bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res); - bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res); - bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res); - bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res); - void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif); - bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res); - void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif); - bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res); - void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif); - bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res); - void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif); - bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res); - void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif); - bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res); - void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif); - bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res); - void cb_UserTopic(Kinova::Api::Base::UserNotification notif); - bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res); - void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif); - bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res); - void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif); - bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res); - void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif); - bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res); - bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res); - bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res); - bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res); - bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res); - bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res); - bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res); - bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res); - bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res); - bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res); - bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res); - bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res); - bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res); - bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res); - bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res); - bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res); - bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res); - bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res); - bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res); - bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res); - bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res); - bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res); - bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res); - bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res); - bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res); - void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif); - bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res); - bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res); - void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif); - bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res); - bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res); - bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res); - bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res); - bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res); - bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res); - void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif); - bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res); - bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res); - void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif); - bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res); - bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res); - bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res); - bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res); - bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res); - bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res); - bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res); - bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res); - bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res); - bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res); - bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res); - bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res); - bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res); - bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res); - bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res); - bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res); - bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res); - bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res); - bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res); - bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res); - bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res); - bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res); - bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res); - bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res); - bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res); - bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res); - bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res); - bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res); - bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res); - bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res); - bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res); - bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res); - bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res); - bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res); - bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res); - bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res); - bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res); - bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res); - bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res); - bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res); - bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res); - bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res); - bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::Base::BaseClient* m_base; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_ConfigurationChangeTopic; - bool m_is_activated_ConfigurationChangeTopic; - ros::Publisher m_pub_MappingInfoTopic; - bool m_is_activated_MappingInfoTopic; - ros::Publisher m_pub_ControlModeTopic; - bool m_is_activated_ControlModeTopic; - ros::Publisher m_pub_OperatingModeTopic; - bool m_is_activated_OperatingModeTopic; - ros::Publisher m_pub_SequenceInfoTopic; - bool m_is_activated_SequenceInfoTopic; - ros::Publisher m_pub_ProtectionZoneTopic; - bool m_is_activated_ProtectionZoneTopic; - ros::Publisher m_pub_UserTopic; - bool m_is_activated_UserTopic; - ros::Publisher m_pub_ControllerTopic; - bool m_is_activated_ControllerTopic; - ros::Publisher m_pub_ActionTopic; - bool m_is_activated_ActionTopic; - ros::Publisher m_pub_RobotEventTopic; - bool m_is_activated_RobotEventTopic; - ros::Publisher m_pub_ServoingModeTopic; - bool m_is_activated_ServoingModeTopic; - ros::Publisher m_pub_FactoryTopic; - bool m_is_activated_FactoryTopic; - ros::Publisher m_pub_NetworkTopic; - bool m_is_activated_NetworkTopic; - ros::Publisher m_pub_ArmStateTopic; - bool m_is_activated_ArmStateTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceCreateUserProfile; - ros::ServiceServer m_serviceUpdateUserProfile; - ros::ServiceServer m_serviceReadUserProfile; - ros::ServiceServer m_serviceDeleteUserProfile; - ros::ServiceServer m_serviceReadAllUserProfiles; - ros::ServiceServer m_serviceReadAllUsers; - ros::ServiceServer m_serviceChangePassword; - ros::ServiceServer m_serviceCreateSequence; - ros::ServiceServer m_serviceUpdateSequence; - ros::ServiceServer m_serviceReadSequence; - ros::ServiceServer m_serviceDeleteSequence; - ros::ServiceServer m_serviceReadAllSequences; - ros::ServiceServer m_servicePlaySequence; - ros::ServiceServer m_servicePlayAdvancedSequence; - ros::ServiceServer m_serviceStopSequence; - ros::ServiceServer m_servicePauseSequence; - ros::ServiceServer m_serviceResumeSequence; - ros::ServiceServer m_serviceCreateProtectionZone; - ros::ServiceServer m_serviceUpdateProtectionZone; - ros::ServiceServer m_serviceReadProtectionZone; - ros::ServiceServer m_serviceDeleteProtectionZone; - ros::ServiceServer m_serviceReadAllProtectionZones; - ros::ServiceServer m_serviceCreateMapping; - ros::ServiceServer m_serviceReadMapping; - ros::ServiceServer m_serviceUpdateMapping; - ros::ServiceServer m_serviceDeleteMapping; - ros::ServiceServer m_serviceReadAllMappings; - ros::ServiceServer m_serviceCreateMap; - ros::ServiceServer m_serviceReadMap; - ros::ServiceServer m_serviceUpdateMap; - ros::ServiceServer m_serviceDeleteMap; - ros::ServiceServer m_serviceReadAllMaps; - ros::ServiceServer m_serviceActivateMap; - ros::ServiceServer m_serviceCreateAction; - ros::ServiceServer m_serviceReadAction; - ros::ServiceServer m_serviceReadAllActions; - ros::ServiceServer m_serviceDeleteAction; - ros::ServiceServer m_serviceUpdateAction; - ros::ServiceServer m_serviceExecuteActionFromReference; - ros::ServiceServer m_serviceExecuteAction; - ros::ServiceServer m_servicePauseAction; - ros::ServiceServer m_serviceStopAction; - ros::ServiceServer m_serviceResumeAction; - ros::ServiceServer m_serviceGetIPv4Configuration; - ros::ServiceServer m_serviceSetIPv4Configuration; - ros::ServiceServer m_serviceSetCommunicationInterfaceEnable; - ros::ServiceServer m_serviceIsCommunicationInterfaceEnable; - ros::ServiceServer m_serviceGetAvailableWifi; - ros::ServiceServer m_serviceGetWifiInformation; - ros::ServiceServer m_serviceAddWifiConfiguration; - ros::ServiceServer m_serviceDeleteWifiConfiguration; - ros::ServiceServer m_serviceGetAllConfiguredWifis; - ros::ServiceServer m_serviceConnectWifi; - ros::ServiceServer m_serviceDisconnectWifi; - ros::ServiceServer m_serviceGetConnectedWifiInformation; - ros::ServiceServer m_serviceBase_Unsubscribe; - ros::ServiceServer m_serviceOnNotificationConfigurationChangeTopic; - ros::ServiceServer m_serviceOnNotificationMappingInfoTopic; - ros::ServiceServer m_serviceOnNotificationControlModeTopic; - ros::ServiceServer m_serviceOnNotificationOperatingModeTopic; - ros::ServiceServer m_serviceOnNotificationSequenceInfoTopic; - ros::ServiceServer m_serviceOnNotificationProtectionZoneTopic; - ros::ServiceServer m_serviceOnNotificationUserTopic; - ros::ServiceServer m_serviceOnNotificationControllerTopic; - ros::ServiceServer m_serviceOnNotificationActionTopic; - ros::ServiceServer m_serviceOnNotificationRobotEventTopic; - ros::ServiceServer m_servicePlayCartesianTrajectory; - ros::ServiceServer m_servicePlayCartesianTrajectoryPosition; - ros::ServiceServer m_servicePlayCartesianTrajectoryOrientation; - ros::ServiceServer m_serviceStop; - ros::ServiceServer m_serviceGetMeasuredCartesianPose; - ros::ServiceServer m_serviceSendWrenchCommand; - ros::ServiceServer m_serviceSendWrenchJoystickCommand; - ros::ServiceServer m_serviceSendTwistJoystickCommand; - ros::ServiceServer m_serviceSendTwistCommand; - ros::ServiceServer m_servicePlayJointTrajectory; - ros::ServiceServer m_servicePlaySelectedJointTrajectory; - ros::ServiceServer m_serviceGetMeasuredJointAngles; - ros::ServiceServer m_serviceSendJointSpeedsCommand; - ros::ServiceServer m_serviceSendSelectedJointSpeedCommand; - ros::ServiceServer m_serviceSendGripperCommand; - ros::ServiceServer m_serviceGetMeasuredGripperMovement; - ros::ServiceServer m_serviceSetAdmittance; - ros::ServiceServer m_serviceSetOperatingMode; - ros::ServiceServer m_serviceApplyEmergencyStop; - ros::ServiceServer m_serviceBase_ClearFaults; - ros::ServiceServer m_serviceBase_GetControlMode; - ros::ServiceServer m_serviceGetOperatingMode; - ros::ServiceServer m_serviceSetServoingMode; - ros::ServiceServer m_serviceGetServoingMode; - ros::ServiceServer m_serviceOnNotificationServoingModeTopic; - ros::ServiceServer m_serviceRestoreFactorySettings; - ros::ServiceServer m_serviceOnNotificationFactoryTopic; - ros::ServiceServer m_serviceGetAllConnectedControllers; - ros::ServiceServer m_serviceGetControllerState; - ros::ServiceServer m_serviceGetActuatorCount; - ros::ServiceServer m_serviceStartWifiScan; - ros::ServiceServer m_serviceGetConfiguredWifi; - ros::ServiceServer m_serviceOnNotificationNetworkTopic; - ros::ServiceServer m_serviceGetArmState; - ros::ServiceServer m_serviceOnNotificationArmStateTopic; - ros::ServiceServer m_serviceGetIPv4Information; - ros::ServiceServer m_serviceSetWifiCountryCode; - ros::ServiceServer m_serviceGetWifiCountryCode; - ros::ServiceServer m_serviceBase_SetCapSenseConfig; - ros::ServiceServer m_serviceBase_GetCapSenseConfig; - ros::ServiceServer m_serviceGetAllJointsSpeedHardLimitation; - ros::ServiceServer m_serviceGetAllJointsTorqueHardLimitation; - ros::ServiceServer m_serviceGetTwistHardLimitation; - ros::ServiceServer m_serviceGetWrenchHardLimitation; - ros::ServiceServer m_serviceSendJointSpeedsJoystickCommand; - ros::ServiceServer m_serviceSendSelectedJointSpeedJoystickCommand; - ros::ServiceServer m_serviceEnableBridge; - ros::ServiceServer m_serviceDisableBridge; - ros::ServiceServer m_serviceGetBridgeList; - ros::ServiceServer m_serviceGetBridgeConfig; - ros::ServiceServer m_servicePlayPreComputedJointTrajectory; - ros::ServiceServer m_serviceGetProductConfiguration; - ros::ServiceServer m_serviceUpdateEndEffectorTypeConfiguration; - ros::ServiceServer m_serviceRestoreFactoryProductConfiguration; - ros::ServiceServer m_serviceGetTrajectoryErrorReport; - ros::ServiceServer m_serviceGetAllJointsSpeedSoftLimitation; - ros::ServiceServer m_serviceGetAllJointsTorqueSoftLimitation; - ros::ServiceServer m_serviceGetTwistSoftLimitation; - ros::ServiceServer m_serviceGetWrenchSoftLimitation; - ros::ServiceServer m_serviceSetControllerConfigurationMode; - ros::ServiceServer m_serviceGetControllerConfigurationMode; - ros::ServiceServer m_serviceStartTeaching; - ros::ServiceServer m_serviceStopTeaching; - ros::ServiceServer m_serviceAddSequenceTasks; - ros::ServiceServer m_serviceUpdateSequenceTask; - ros::ServiceServer m_serviceSwapSequenceTasks; - ros::ServiceServer m_serviceReadSequenceTask; - ros::ServiceServer m_serviceReadAllSequenceTasks; - ros::ServiceServer m_serviceDeleteSequenceTask; - ros::ServiceServer m_serviceDeleteAllSequenceTasks; - ros::ServiceServer m_serviceTakeSnapshot; - ros::ServiceServer m_serviceGetFirmwareBundleVersions; - ros::ServiceServer m_serviceMoveSequenceTask; - ros::ServiceServer m_serviceDuplicateMapping; - ros::ServiceServer m_serviceDuplicateMap; - ros::ServiceServer m_serviceSetControllerConfiguration; - ros::ServiceServer m_serviceGetControllerConfiguration; - ros::ServiceServer m_serviceGetAllControllerConfigurations; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/controlconfig_services.h deleted file mode 100644 index 5483d206..00000000 --- a/kortex_driver/include/kortex_driver/generated/controlconfig_services.h +++ /dev/null @@ -1,150 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_CONTROLCONFIG_SERVICES_H_ -#define _KORTEX_CONTROLCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/SetGravityVector.h" -#include "kortex_driver/GetGravityVector.h" -#include "kortex_driver/SetPayloadInformation.h" -#include "kortex_driver/GetPayloadInformation.h" -#include "kortex_driver/SetToolConfiguration.h" -#include "kortex_driver/GetToolConfiguration.h" -#include "kortex_driver/OnNotificationControlConfigurationTopic.h" -#include "kortex_driver/ControlConfigurationNotification.h" -#include "kortex_driver/ControlConfig_Unsubscribe.h" -#include "kortex_driver/SetCartesianReferenceFrame.h" -#include "kortex_driver/GetCartesianReferenceFrame.h" -#include "kortex_driver/ControlConfig_GetControlMode.h" -#include "kortex_driver/SetJointSpeedSoftLimits.h" -#include "kortex_driver/SetTwistLinearSoftLimit.h" -#include "kortex_driver/SetTwistAngularSoftLimit.h" -#include "kortex_driver/SetJointAccelerationSoftLimits.h" -#include "kortex_driver/GetKinematicHardLimits.h" -#include "kortex_driver/GetKinematicSoftLimits.h" -#include "kortex_driver/GetAllKinematicSoftLimits.h" -#include "kortex_driver/SetDesiredLinearTwist.h" -#include "kortex_driver/SetDesiredAngularTwist.h" -#include "kortex_driver/SetDesiredJointSpeeds.h" -#include "kortex_driver/GetDesiredSpeeds.h" -#include "kortex_driver/ResetGravityVector.h" -#include "kortex_driver/ResetPayloadInformation.h" -#include "kortex_driver/ResetToolConfiguration.h" -#include "kortex_driver/ResetJointSpeedSoftLimits.h" -#include "kortex_driver/ResetTwistLinearSoftLimit.h" -#include "kortex_driver/ResetTwistAngularSoftLimit.h" -#include "kortex_driver/ResetJointAccelerationSoftLimits.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class ControlConfigServices -{ - public: - ControlConfigServices(ros::NodeHandle& n, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res); - bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res); - bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res); - bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res); - bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res); - bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res); - bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res); - void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif); - bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res); - bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res); - bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res); - bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res); - bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res); - bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res); - bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res); - bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res); - bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res); - bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res); - bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res); - bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res); - bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res); - bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res); - bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res); - bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res); - bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res); - bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res); - bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res); - bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res); - bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res); - bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::ControlConfig::ControlConfigClient* m_controlconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_ControlConfigurationTopic; - bool m_is_activated_ControlConfigurationTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceSetGravityVector; - ros::ServiceServer m_serviceGetGravityVector; - ros::ServiceServer m_serviceSetPayloadInformation; - ros::ServiceServer m_serviceGetPayloadInformation; - ros::ServiceServer m_serviceSetToolConfiguration; - ros::ServiceServer m_serviceGetToolConfiguration; - ros::ServiceServer m_serviceOnNotificationControlConfigurationTopic; - ros::ServiceServer m_serviceControlConfig_Unsubscribe; - ros::ServiceServer m_serviceSetCartesianReferenceFrame; - ros::ServiceServer m_serviceGetCartesianReferenceFrame; - ros::ServiceServer m_serviceControlConfig_GetControlMode; - ros::ServiceServer m_serviceSetJointSpeedSoftLimits; - ros::ServiceServer m_serviceSetTwistLinearSoftLimit; - ros::ServiceServer m_serviceSetTwistAngularSoftLimit; - ros::ServiceServer m_serviceSetJointAccelerationSoftLimits; - ros::ServiceServer m_serviceGetKinematicHardLimits; - ros::ServiceServer m_serviceGetKinematicSoftLimits; - ros::ServiceServer m_serviceGetAllKinematicSoftLimits; - ros::ServiceServer m_serviceSetDesiredLinearTwist; - ros::ServiceServer m_serviceSetDesiredAngularTwist; - ros::ServiceServer m_serviceSetDesiredJointSpeeds; - ros::ServiceServer m_serviceGetDesiredSpeeds; - ros::ServiceServer m_serviceResetGravityVector; - ros::ServiceServer m_serviceResetPayloadInformation; - ros::ServiceServer m_serviceResetToolConfiguration; - ros::ServiceServer m_serviceResetJointSpeedSoftLimits; - ros::ServiceServer m_serviceResetTwistLinearSoftLimit; - ros::ServiceServer m_serviceResetTwistAngularSoftLimit; - ros::ServiceServer m_serviceResetJointAccelerationSoftLimits; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/deviceconfig_services.h deleted file mode 100644 index f80caaf0..00000000 --- a/kortex_driver/include/kortex_driver/generated/deviceconfig_services.h +++ /dev/null @@ -1,159 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICECONFIG_SERVICES_H_ -#define _KORTEX_DEVICECONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/GetRunMode.h" -#include "kortex_driver/SetRunMode.h" -#include "kortex_driver/GetDeviceType.h" -#include "kortex_driver/GetFirmwareVersion.h" -#include "kortex_driver/GetBootloaderVersion.h" -#include "kortex_driver/GetModelNumber.h" -#include "kortex_driver/GetPartNumber.h" -#include "kortex_driver/GetSerialNumber.h" -#include "kortex_driver/GetMACAddress.h" -#include "kortex_driver/GetIPv4Settings.h" -#include "kortex_driver/SetIPv4Settings.h" -#include "kortex_driver/GetPartNumberRevision.h" -#include "kortex_driver/RebootRequest.h" -#include "kortex_driver/SetSafetyEnable.h" -#include "kortex_driver/SetSafetyErrorThreshold.h" -#include "kortex_driver/SetSafetyWarningThreshold.h" -#include "kortex_driver/SetSafetyConfiguration.h" -#include "kortex_driver/GetSafetyConfiguration.h" -#include "kortex_driver/GetSafetyInformation.h" -#include "kortex_driver/GetSafetyEnable.h" -#include "kortex_driver/GetSafetyStatus.h" -#include "kortex_driver/ClearAllSafetyStatus.h" -#include "kortex_driver/ClearSafetyStatus.h" -#include "kortex_driver/GetAllSafetyConfiguration.h" -#include "kortex_driver/GetAllSafetyInformation.h" -#include "kortex_driver/ResetSafetyDefaults.h" -#include "kortex_driver/OnNotificationSafetyTopic.h" -#include "kortex_driver/SafetyNotification.h" -#include "kortex_driver/ExecuteCalibration.h" -#include "kortex_driver/GetCalibrationResult.h" -#include "kortex_driver/StopCalibration.h" -#include "kortex_driver/DeviceConfig_SetCapSenseConfig.h" -#include "kortex_driver/DeviceConfig_GetCapSenseConfig.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class DeviceConfigServices -{ - public: - DeviceConfigServices(ros::NodeHandle& n, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res); - bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res); - bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res); - bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res); - bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res); - bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res); - bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res); - bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res); - bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res); - bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res); - bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res); - bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res); - bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res); - bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res); - bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res); - bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res); - bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res); - bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res); - bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res); - bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res); - bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res); - bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res); - bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res); - bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res); - bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res); - bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res); - bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res); - void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif); - bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res); - bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res); - bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res); - bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res); - bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::DeviceConfig::DeviceConfigClient* m_deviceconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_SafetyTopic; - bool m_is_activated_SafetyTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceGetRunMode; - ros::ServiceServer m_serviceSetRunMode; - ros::ServiceServer m_serviceGetDeviceType; - ros::ServiceServer m_serviceGetFirmwareVersion; - ros::ServiceServer m_serviceGetBootloaderVersion; - ros::ServiceServer m_serviceGetModelNumber; - ros::ServiceServer m_serviceGetPartNumber; - ros::ServiceServer m_serviceGetSerialNumber; - ros::ServiceServer m_serviceGetMACAddress; - ros::ServiceServer m_serviceGetIPv4Settings; - ros::ServiceServer m_serviceSetIPv4Settings; - ros::ServiceServer m_serviceGetPartNumberRevision; - ros::ServiceServer m_serviceRebootRequest; - ros::ServiceServer m_serviceSetSafetyEnable; - ros::ServiceServer m_serviceSetSafetyErrorThreshold; - ros::ServiceServer m_serviceSetSafetyWarningThreshold; - ros::ServiceServer m_serviceSetSafetyConfiguration; - ros::ServiceServer m_serviceGetSafetyConfiguration; - ros::ServiceServer m_serviceGetSafetyInformation; - ros::ServiceServer m_serviceGetSafetyEnable; - ros::ServiceServer m_serviceGetSafetyStatus; - ros::ServiceServer m_serviceClearAllSafetyStatus; - ros::ServiceServer m_serviceClearSafetyStatus; - ros::ServiceServer m_serviceGetAllSafetyConfiguration; - ros::ServiceServer m_serviceGetAllSafetyInformation; - ros::ServiceServer m_serviceResetSafetyDefaults; - ros::ServiceServer m_serviceOnNotificationSafetyTopic; - ros::ServiceServer m_serviceExecuteCalibration; - ros::ServiceServer m_serviceGetCalibrationResult; - ros::ServiceServer m_serviceStopCalibration; - ros::ServiceServer m_serviceDeviceConfig_SetCapSenseConfig; - ros::ServiceServer m_serviceDeviceConfig_GetCapSenseConfig; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h b/kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h deleted file mode 100644 index 352de2e7..00000000 --- a/kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ -#define _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include - -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" - - -#include "kortex_driver/DeviceHandles.h" - - -int ToProtoData(kortex_driver::DeviceHandles input, Kinova::Api::DeviceManager::DeviceHandles *output); - -#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h b/kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h deleted file mode 100644 index 4e5359b7..00000000 --- a/kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ -#define _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include - -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" - - -#include "kortex_driver/DeviceHandles.h" - - -int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::DeviceHandles &output); - -#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/devicemanager_services.h deleted file mode 100644 index b5d016a3..00000000 --- a/kortex_driver/include/kortex_driver/generated/devicemanager_services.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICEMANAGER_SERVICES_H_ -#define _KORTEX_DEVICEMANAGER_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/ReadAllDevices.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class DeviceManagerServices -{ - public: - DeviceManagerServices(ros::NodeHandle& n, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::DeviceManager::DeviceManagerClient* m_devicemanager; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceReadAllDevices; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h deleted file mode 100644 index 34e135e6..00000000 --- a/kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_INTERCONNECTCONFIG_SERVICES_H_ -#define _KORTEX_INTERCONNECTCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/GetUARTConfiguration.h" -#include "kortex_driver/SetUARTConfiguration.h" -#include "kortex_driver/GetEthernetConfiguration.h" -#include "kortex_driver/SetEthernetConfiguration.h" -#include "kortex_driver/GetGPIOConfiguration.h" -#include "kortex_driver/SetGPIOConfiguration.h" -#include "kortex_driver/GetGPIOState.h" -#include "kortex_driver/SetGPIOState.h" -#include "kortex_driver/GetI2CConfiguration.h" -#include "kortex_driver/SetI2CConfiguration.h" -#include "kortex_driver/I2CRead.h" -#include "kortex_driver/I2CReadRegister.h" -#include "kortex_driver/I2CWrite.h" -#include "kortex_driver/I2CWriteRegister.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class InterconnectConfigServices -{ - public: - InterconnectConfigServices(ros::NodeHandle& n, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res); - bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res); - bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res); - bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res); - bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res); - bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res); - bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res); - bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res); - bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res); - bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res); - bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res); - bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res); - bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res); - bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::InterconnectConfig::InterconnectConfigClient* m_interconnectconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceGetUARTConfiguration; - ros::ServiceServer m_serviceSetUARTConfiguration; - ros::ServiceServer m_serviceGetEthernetConfiguration; - ros::ServiceServer m_serviceSetEthernetConfiguration; - ros::ServiceServer m_serviceGetGPIOConfiguration; - ros::ServiceServer m_serviceSetGPIOConfiguration; - ros::ServiceServer m_serviceGetGPIOState; - ros::ServiceServer m_serviceSetGPIOState; - ros::ServiceServer m_serviceGetI2CConfiguration; - ros::ServiceServer m_serviceSetI2CConfiguration; - ros::ServiceServer m_serviceI2CRead; - ros::ServiceServer m_serviceI2CReadRegister; - ros::ServiceServer m_serviceI2CWrite; - ros::ServiceServer m_serviceI2CWriteRegister; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h new file mode 100644 index 00000000..0aedc895 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h @@ -0,0 +1,111 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ACTUATORCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_ACTUATORCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/GetAxisOffsets.h" +#include "kortex_driver/SetAxisOffsets.h" +#include "kortex_driver/SetTorqueOffset.h" +#include "kortex_driver/ActuatorConfig_GetControlMode.h" +#include "kortex_driver/SetControlMode.h" +#include "kortex_driver/GetActivatedControlLoop.h" +#include "kortex_driver/SetActivatedControlLoop.h" +#include "kortex_driver/GetControlLoopParameters.h" +#include "kortex_driver/SetControlLoopParameters.h" +#include "kortex_driver/SelectCustomData.h" +#include "kortex_driver/GetSelectedCustomData.h" +#include "kortex_driver/SetCommandMode.h" +#include "kortex_driver/ActuatorConfig_ClearFaults.h" +#include "kortex_driver/SetServoing.h" +#include "kortex_driver/MoveToPosition.h" +#include "kortex_driver/GetCommandMode.h" +#include "kortex_driver/GetServoing.h" +#include "kortex_driver/GetTorqueOffset.h" +#include "kortex_driver/SetCoggingFeedforwardMode.h" +#include "kortex_driver/GetCoggingFeedforwardMode.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IActuatorConfigServices +{ + public: + IActuatorConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) = 0; + virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) = 0; + virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) = 0; + virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) = 0; + virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) = 0; + virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) = 0; + virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) = 0; + virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) = 0; + virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) = 0; + virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) = 0; + virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) = 0; + virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) = 0; + virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) = 0; + virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) = 0; + virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) = 0; + virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) = 0; + virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) = 0; + virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) = 0; + virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) = 0; + virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceGetAxisOffsets; + ros::ServiceServer m_serviceSetAxisOffsets; + ros::ServiceServer m_serviceSetTorqueOffset; + ros::ServiceServer m_serviceActuatorConfig_GetControlMode; + ros::ServiceServer m_serviceSetControlMode; + ros::ServiceServer m_serviceGetActivatedControlLoop; + ros::ServiceServer m_serviceSetActivatedControlLoop; + ros::ServiceServer m_serviceGetControlLoopParameters; + ros::ServiceServer m_serviceSetControlLoopParameters; + ros::ServiceServer m_serviceSelectCustomData; + ros::ServiceServer m_serviceGetSelectedCustomData; + ros::ServiceServer m_serviceSetCommandMode; + ros::ServiceServer m_serviceActuatorConfig_ClearFaults; + ros::ServiceServer m_serviceSetServoing; + ros::ServiceServer m_serviceMoveToPosition; + ros::ServiceServer m_serviceGetCommandMode; + ros::ServiceServer m_serviceGetServoing; + ros::ServiceServer m_serviceGetTorqueOffset; + ros::ServiceServer m_serviceSetCoggingFeedforwardMode; + ros::ServiceServer m_serviceGetCoggingFeedforwardMode; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h new file mode 100644 index 00000000..1df8686a --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h @@ -0,0 +1,539 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BASE_SERVICES_INTERFACE_H_ +#define _KORTEX_BASE_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/CreateUserProfile.h" +#include "kortex_driver/UpdateUserProfile.h" +#include "kortex_driver/ReadUserProfile.h" +#include "kortex_driver/DeleteUserProfile.h" +#include "kortex_driver/ReadAllUserProfiles.h" +#include "kortex_driver/ReadAllUsers.h" +#include "kortex_driver/ChangePassword.h" +#include "kortex_driver/CreateSequence.h" +#include "kortex_driver/UpdateSequence.h" +#include "kortex_driver/ReadSequence.h" +#include "kortex_driver/DeleteSequence.h" +#include "kortex_driver/ReadAllSequences.h" +#include "kortex_driver/PlaySequence.h" +#include "kortex_driver/PlayAdvancedSequence.h" +#include "kortex_driver/StopSequence.h" +#include "kortex_driver/PauseSequence.h" +#include "kortex_driver/ResumeSequence.h" +#include "kortex_driver/CreateProtectionZone.h" +#include "kortex_driver/UpdateProtectionZone.h" +#include "kortex_driver/ReadProtectionZone.h" +#include "kortex_driver/DeleteProtectionZone.h" +#include "kortex_driver/ReadAllProtectionZones.h" +#include "kortex_driver/CreateMapping.h" +#include "kortex_driver/ReadMapping.h" +#include "kortex_driver/UpdateMapping.h" +#include "kortex_driver/DeleteMapping.h" +#include "kortex_driver/ReadAllMappings.h" +#include "kortex_driver/CreateMap.h" +#include "kortex_driver/ReadMap.h" +#include "kortex_driver/UpdateMap.h" +#include "kortex_driver/DeleteMap.h" +#include "kortex_driver/ReadAllMaps.h" +#include "kortex_driver/ActivateMap.h" +#include "kortex_driver/CreateAction.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ReadAllActions.h" +#include "kortex_driver/DeleteAction.h" +#include "kortex_driver/UpdateAction.h" +#include "kortex_driver/ExecuteActionFromReference.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/PauseAction.h" +#include "kortex_driver/StopAction.h" +#include "kortex_driver/ResumeAction.h" +#include "kortex_driver/GetIPv4Configuration.h" +#include "kortex_driver/SetIPv4Configuration.h" +#include "kortex_driver/SetCommunicationInterfaceEnable.h" +#include "kortex_driver/IsCommunicationInterfaceEnable.h" +#include "kortex_driver/GetAvailableWifi.h" +#include "kortex_driver/GetWifiInformation.h" +#include "kortex_driver/AddWifiConfiguration.h" +#include "kortex_driver/DeleteWifiConfiguration.h" +#include "kortex_driver/GetAllConfiguredWifis.h" +#include "kortex_driver/ConnectWifi.h" +#include "kortex_driver/DisconnectWifi.h" +#include "kortex_driver/GetConnectedWifiInformation.h" +#include "kortex_driver/Base_Unsubscribe.h" +#include "kortex_driver/OnNotificationConfigurationChangeTopic.h" +#include "kortex_driver/ConfigurationChangeNotification.h" +#include "kortex_driver/OnNotificationMappingInfoTopic.h" +#include "kortex_driver/MappingInfoNotification.h" +#include "kortex_driver/OnNotificationControlModeTopic.h" +#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/OnNotificationOperatingModeTopic.h" +#include "kortex_driver/OperatingModeNotification.h" +#include "kortex_driver/OnNotificationSequenceInfoTopic.h" +#include "kortex_driver/SequenceInfoNotification.h" +#include "kortex_driver/OnNotificationProtectionZoneTopic.h" +#include "kortex_driver/ProtectionZoneNotification.h" +#include "kortex_driver/OnNotificationUserTopic.h" +#include "kortex_driver/UserNotification.h" +#include "kortex_driver/OnNotificationControllerTopic.h" +#include "kortex_driver/ControllerNotification.h" +#include "kortex_driver/OnNotificationActionTopic.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/OnNotificationRobotEventTopic.h" +#include "kortex_driver/RobotEventNotification.h" +#include "kortex_driver/PlayCartesianTrajectory.h" +#include "kortex_driver/PlayCartesianTrajectoryPosition.h" +#include "kortex_driver/PlayCartesianTrajectoryOrientation.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/GetMeasuredCartesianPose.h" +#include "kortex_driver/SendWrenchCommand.h" +#include "kortex_driver/SendWrenchJoystickCommand.h" +#include "kortex_driver/SendTwistJoystickCommand.h" +#include "kortex_driver/SendTwistCommand.h" +#include "kortex_driver/PlayJointTrajectory.h" +#include "kortex_driver/PlaySelectedJointTrajectory.h" +#include "kortex_driver/GetMeasuredJointAngles.h" +#include "kortex_driver/SendJointSpeedsCommand.h" +#include "kortex_driver/SendSelectedJointSpeedCommand.h" +#include "kortex_driver/SendGripperCommand.h" +#include "kortex_driver/GetMeasuredGripperMovement.h" +#include "kortex_driver/SetAdmittance.h" +#include "kortex_driver/SetOperatingMode.h" +#include "kortex_driver/ApplyEmergencyStop.h" +#include "kortex_driver/Base_ClearFaults.h" +#include "kortex_driver/Base_GetControlMode.h" +#include "kortex_driver/GetOperatingMode.h" +#include "kortex_driver/SetServoingMode.h" +#include "kortex_driver/GetServoingMode.h" +#include "kortex_driver/OnNotificationServoingModeTopic.h" +#include "kortex_driver/ServoingModeNotification.h" +#include "kortex_driver/RestoreFactorySettings.h" +#include "kortex_driver/OnNotificationFactoryTopic.h" +#include "kortex_driver/FactoryNotification.h" +#include "kortex_driver/GetAllConnectedControllers.h" +#include "kortex_driver/GetControllerState.h" +#include "kortex_driver/GetActuatorCount.h" +#include "kortex_driver/StartWifiScan.h" +#include "kortex_driver/GetConfiguredWifi.h" +#include "kortex_driver/OnNotificationNetworkTopic.h" +#include "kortex_driver/NetworkNotification.h" +#include "kortex_driver/GetArmState.h" +#include "kortex_driver/OnNotificationArmStateTopic.h" +#include "kortex_driver/ArmStateNotification.h" +#include "kortex_driver/GetIPv4Information.h" +#include "kortex_driver/SetWifiCountryCode.h" +#include "kortex_driver/GetWifiCountryCode.h" +#include "kortex_driver/Base_SetCapSenseConfig.h" +#include "kortex_driver/Base_GetCapSenseConfig.h" +#include "kortex_driver/GetAllJointsSpeedHardLimitation.h" +#include "kortex_driver/GetAllJointsTorqueHardLimitation.h" +#include "kortex_driver/GetTwistHardLimitation.h" +#include "kortex_driver/GetWrenchHardLimitation.h" +#include "kortex_driver/SendJointSpeedsJoystickCommand.h" +#include "kortex_driver/SendSelectedJointSpeedJoystickCommand.h" +#include "kortex_driver/EnableBridge.h" +#include "kortex_driver/DisableBridge.h" +#include "kortex_driver/GetBridgeList.h" +#include "kortex_driver/GetBridgeConfig.h" +#include "kortex_driver/PlayPreComputedJointTrajectory.h" +#include "kortex_driver/GetProductConfiguration.h" +#include "kortex_driver/UpdateEndEffectorTypeConfiguration.h" +#include "kortex_driver/RestoreFactoryProductConfiguration.h" +#include "kortex_driver/GetTrajectoryErrorReport.h" +#include "kortex_driver/GetAllJointsSpeedSoftLimitation.h" +#include "kortex_driver/GetAllJointsTorqueSoftLimitation.h" +#include "kortex_driver/GetTwistSoftLimitation.h" +#include "kortex_driver/GetWrenchSoftLimitation.h" +#include "kortex_driver/SetControllerConfigurationMode.h" +#include "kortex_driver/GetControllerConfigurationMode.h" +#include "kortex_driver/StartTeaching.h" +#include "kortex_driver/StopTeaching.h" +#include "kortex_driver/AddSequenceTasks.h" +#include "kortex_driver/UpdateSequenceTask.h" +#include "kortex_driver/SwapSequenceTasks.h" +#include "kortex_driver/ReadSequenceTask.h" +#include "kortex_driver/ReadAllSequenceTasks.h" +#include "kortex_driver/DeleteSequenceTask.h" +#include "kortex_driver/DeleteAllSequenceTasks.h" +#include "kortex_driver/TakeSnapshot.h" +#include "kortex_driver/GetFirmwareBundleVersions.h" +#include "kortex_driver/MoveSequenceTask.h" +#include "kortex_driver/DuplicateMapping.h" +#include "kortex_driver/DuplicateMap.h" +#include "kortex_driver/SetControllerConfiguration.h" +#include "kortex_driver/GetControllerConfiguration.h" +#include "kortex_driver/GetAllControllerConfigurations.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IBaseServices +{ + public: + IBaseServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) = 0; + virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) = 0; + virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) = 0; + virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) = 0; + virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) = 0; + virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) = 0; + virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) = 0; + virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) = 0; + virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) = 0; + virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) = 0; + virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) = 0; + virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) = 0; + virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) = 0; + virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) = 0; + virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) = 0; + virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) = 0; + virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) = 0; + virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) = 0; + virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) = 0; + virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) = 0; + virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) = 0; + virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) = 0; + virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) = 0; + virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) = 0; + virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) = 0; + virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) = 0; + virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) = 0; + virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) = 0; + virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) = 0; + virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) = 0; + virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) = 0; + virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) = 0; + virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) = 0; + virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) = 0; + virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) = 0; + virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) = 0; + virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) = 0; + virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) = 0; + virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) = 0; + virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) = 0; + virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) = 0; + virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) = 0; + virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) = 0; + virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) = 0; + virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) = 0; + virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) = 0; + virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) = 0; + virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) = 0; + virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) = 0; + virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) = 0; + virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) = 0; + virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) = 0; + virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) = 0; + virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) = 0; + virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) = 0; + virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) = 0; + virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) = 0; + virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) = 0; + virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) = 0; + virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) = 0; + virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) = 0; + virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) = 0; + virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) = 0; + virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) = 0; + virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) = 0; + virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) = 0; + virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) = 0; + virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) = 0; + virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) = 0; + virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) = 0; + virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) = 0; + virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) = 0; + virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) = 0; + virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) = 0; + virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) = 0; + virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) = 0; + virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) = 0; + virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) = 0; + virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) = 0; + virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) = 0; + virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) = 0; + virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) = 0; + virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) = 0; + virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) = 0; + virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) = 0; + virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) = 0; + virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) = 0; + virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) = 0; + virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) = 0; + virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) = 0; + virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) = 0; + virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) = 0; + virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) = 0; + virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) = 0; + virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) = 0; + virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) = 0; + virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) = 0; + virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) = 0; + virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) = 0; + virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) = 0; + virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) = 0; + virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) = 0; + virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) = 0; + virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) = 0; + virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) = 0; + virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) = 0; + virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) = 0; + virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) = 0; + virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) = 0; + virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) = 0; + virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) = 0; + virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) = 0; + virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) = 0; + virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) = 0; + virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) = 0; + virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) = 0; + virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) = 0; + virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) = 0; + virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) = 0; + virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) = 0; + virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) = 0; + virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) = 0; + virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) = 0; + virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) = 0; + virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) = 0; + virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) = 0; + virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) = 0; + virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) = 0; + virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) = 0; + virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) = 0; + virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) = 0; + virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) = 0; + virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) = 0; + virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) = 0; + virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) = 0; + virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) = 0; + virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) = 0; + virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) = 0; + virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) = 0; + virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) = 0; + virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) = 0; + virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) = 0; + virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) = 0; + virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) = 0; + virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) = 0; + virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) = 0; + virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) = 0; + virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) = 0; + virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) = 0; + virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) = 0; + virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) = 0; + virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) = 0; + virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) = 0; + virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) = 0; + virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) = 0; + virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) = 0; + virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) = 0; + virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_ConfigurationChangeTopic; + bool m_is_activated_ConfigurationChangeTopic; + ros::Publisher m_pub_MappingInfoTopic; + bool m_is_activated_MappingInfoTopic; + ros::Publisher m_pub_ControlModeTopic; + bool m_is_activated_ControlModeTopic; + ros::Publisher m_pub_OperatingModeTopic; + bool m_is_activated_OperatingModeTopic; + ros::Publisher m_pub_SequenceInfoTopic; + bool m_is_activated_SequenceInfoTopic; + ros::Publisher m_pub_ProtectionZoneTopic; + bool m_is_activated_ProtectionZoneTopic; + ros::Publisher m_pub_UserTopic; + bool m_is_activated_UserTopic; + ros::Publisher m_pub_ControllerTopic; + bool m_is_activated_ControllerTopic; + ros::Publisher m_pub_ActionTopic; + bool m_is_activated_ActionTopic; + ros::Publisher m_pub_RobotEventTopic; + bool m_is_activated_RobotEventTopic; + ros::Publisher m_pub_ServoingModeTopic; + bool m_is_activated_ServoingModeTopic; + ros::Publisher m_pub_FactoryTopic; + bool m_is_activated_FactoryTopic; + ros::Publisher m_pub_NetworkTopic; + bool m_is_activated_NetworkTopic; + ros::Publisher m_pub_ArmStateTopic; + bool m_is_activated_ArmStateTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceCreateUserProfile; + ros::ServiceServer m_serviceUpdateUserProfile; + ros::ServiceServer m_serviceReadUserProfile; + ros::ServiceServer m_serviceDeleteUserProfile; + ros::ServiceServer m_serviceReadAllUserProfiles; + ros::ServiceServer m_serviceReadAllUsers; + ros::ServiceServer m_serviceChangePassword; + ros::ServiceServer m_serviceCreateSequence; + ros::ServiceServer m_serviceUpdateSequence; + ros::ServiceServer m_serviceReadSequence; + ros::ServiceServer m_serviceDeleteSequence; + ros::ServiceServer m_serviceReadAllSequences; + ros::ServiceServer m_servicePlaySequence; + ros::ServiceServer m_servicePlayAdvancedSequence; + ros::ServiceServer m_serviceStopSequence; + ros::ServiceServer m_servicePauseSequence; + ros::ServiceServer m_serviceResumeSequence; + ros::ServiceServer m_serviceCreateProtectionZone; + ros::ServiceServer m_serviceUpdateProtectionZone; + ros::ServiceServer m_serviceReadProtectionZone; + ros::ServiceServer m_serviceDeleteProtectionZone; + ros::ServiceServer m_serviceReadAllProtectionZones; + ros::ServiceServer m_serviceCreateMapping; + ros::ServiceServer m_serviceReadMapping; + ros::ServiceServer m_serviceUpdateMapping; + ros::ServiceServer m_serviceDeleteMapping; + ros::ServiceServer m_serviceReadAllMappings; + ros::ServiceServer m_serviceCreateMap; + ros::ServiceServer m_serviceReadMap; + ros::ServiceServer m_serviceUpdateMap; + ros::ServiceServer m_serviceDeleteMap; + ros::ServiceServer m_serviceReadAllMaps; + ros::ServiceServer m_serviceActivateMap; + ros::ServiceServer m_serviceCreateAction; + ros::ServiceServer m_serviceReadAction; + ros::ServiceServer m_serviceReadAllActions; + ros::ServiceServer m_serviceDeleteAction; + ros::ServiceServer m_serviceUpdateAction; + ros::ServiceServer m_serviceExecuteActionFromReference; + ros::ServiceServer m_serviceExecuteAction; + ros::ServiceServer m_servicePauseAction; + ros::ServiceServer m_serviceStopAction; + ros::ServiceServer m_serviceResumeAction; + ros::ServiceServer m_serviceGetIPv4Configuration; + ros::ServiceServer m_serviceSetIPv4Configuration; + ros::ServiceServer m_serviceSetCommunicationInterfaceEnable; + ros::ServiceServer m_serviceIsCommunicationInterfaceEnable; + ros::ServiceServer m_serviceGetAvailableWifi; + ros::ServiceServer m_serviceGetWifiInformation; + ros::ServiceServer m_serviceAddWifiConfiguration; + ros::ServiceServer m_serviceDeleteWifiConfiguration; + ros::ServiceServer m_serviceGetAllConfiguredWifis; + ros::ServiceServer m_serviceConnectWifi; + ros::ServiceServer m_serviceDisconnectWifi; + ros::ServiceServer m_serviceGetConnectedWifiInformation; + ros::ServiceServer m_serviceBase_Unsubscribe; + ros::ServiceServer m_serviceOnNotificationConfigurationChangeTopic; + ros::ServiceServer m_serviceOnNotificationMappingInfoTopic; + ros::ServiceServer m_serviceOnNotificationControlModeTopic; + ros::ServiceServer m_serviceOnNotificationOperatingModeTopic; + ros::ServiceServer m_serviceOnNotificationSequenceInfoTopic; + ros::ServiceServer m_serviceOnNotificationProtectionZoneTopic; + ros::ServiceServer m_serviceOnNotificationUserTopic; + ros::ServiceServer m_serviceOnNotificationControllerTopic; + ros::ServiceServer m_serviceOnNotificationActionTopic; + ros::ServiceServer m_serviceOnNotificationRobotEventTopic; + ros::ServiceServer m_servicePlayCartesianTrajectory; + ros::ServiceServer m_servicePlayCartesianTrajectoryPosition; + ros::ServiceServer m_servicePlayCartesianTrajectoryOrientation; + ros::ServiceServer m_serviceStop; + ros::ServiceServer m_serviceGetMeasuredCartesianPose; + ros::ServiceServer m_serviceSendWrenchCommand; + ros::ServiceServer m_serviceSendWrenchJoystickCommand; + ros::ServiceServer m_serviceSendTwistJoystickCommand; + ros::ServiceServer m_serviceSendTwistCommand; + ros::ServiceServer m_servicePlayJointTrajectory; + ros::ServiceServer m_servicePlaySelectedJointTrajectory; + ros::ServiceServer m_serviceGetMeasuredJointAngles; + ros::ServiceServer m_serviceSendJointSpeedsCommand; + ros::ServiceServer m_serviceSendSelectedJointSpeedCommand; + ros::ServiceServer m_serviceSendGripperCommand; + ros::ServiceServer m_serviceGetMeasuredGripperMovement; + ros::ServiceServer m_serviceSetAdmittance; + ros::ServiceServer m_serviceSetOperatingMode; + ros::ServiceServer m_serviceApplyEmergencyStop; + ros::ServiceServer m_serviceBase_ClearFaults; + ros::ServiceServer m_serviceBase_GetControlMode; + ros::ServiceServer m_serviceGetOperatingMode; + ros::ServiceServer m_serviceSetServoingMode; + ros::ServiceServer m_serviceGetServoingMode; + ros::ServiceServer m_serviceOnNotificationServoingModeTopic; + ros::ServiceServer m_serviceRestoreFactorySettings; + ros::ServiceServer m_serviceOnNotificationFactoryTopic; + ros::ServiceServer m_serviceGetAllConnectedControllers; + ros::ServiceServer m_serviceGetControllerState; + ros::ServiceServer m_serviceGetActuatorCount; + ros::ServiceServer m_serviceStartWifiScan; + ros::ServiceServer m_serviceGetConfiguredWifi; + ros::ServiceServer m_serviceOnNotificationNetworkTopic; + ros::ServiceServer m_serviceGetArmState; + ros::ServiceServer m_serviceOnNotificationArmStateTopic; + ros::ServiceServer m_serviceGetIPv4Information; + ros::ServiceServer m_serviceSetWifiCountryCode; + ros::ServiceServer m_serviceGetWifiCountryCode; + ros::ServiceServer m_serviceBase_SetCapSenseConfig; + ros::ServiceServer m_serviceBase_GetCapSenseConfig; + ros::ServiceServer m_serviceGetAllJointsSpeedHardLimitation; + ros::ServiceServer m_serviceGetAllJointsTorqueHardLimitation; + ros::ServiceServer m_serviceGetTwistHardLimitation; + ros::ServiceServer m_serviceGetWrenchHardLimitation; + ros::ServiceServer m_serviceSendJointSpeedsJoystickCommand; + ros::ServiceServer m_serviceSendSelectedJointSpeedJoystickCommand; + ros::ServiceServer m_serviceEnableBridge; + ros::ServiceServer m_serviceDisableBridge; + ros::ServiceServer m_serviceGetBridgeList; + ros::ServiceServer m_serviceGetBridgeConfig; + ros::ServiceServer m_servicePlayPreComputedJointTrajectory; + ros::ServiceServer m_serviceGetProductConfiguration; + ros::ServiceServer m_serviceUpdateEndEffectorTypeConfiguration; + ros::ServiceServer m_serviceRestoreFactoryProductConfiguration; + ros::ServiceServer m_serviceGetTrajectoryErrorReport; + ros::ServiceServer m_serviceGetAllJointsSpeedSoftLimitation; + ros::ServiceServer m_serviceGetAllJointsTorqueSoftLimitation; + ros::ServiceServer m_serviceGetTwistSoftLimitation; + ros::ServiceServer m_serviceGetWrenchSoftLimitation; + ros::ServiceServer m_serviceSetControllerConfigurationMode; + ros::ServiceServer m_serviceGetControllerConfigurationMode; + ros::ServiceServer m_serviceStartTeaching; + ros::ServiceServer m_serviceStopTeaching; + ros::ServiceServer m_serviceAddSequenceTasks; + ros::ServiceServer m_serviceUpdateSequenceTask; + ros::ServiceServer m_serviceSwapSequenceTasks; + ros::ServiceServer m_serviceReadSequenceTask; + ros::ServiceServer m_serviceReadAllSequenceTasks; + ros::ServiceServer m_serviceDeleteSequenceTask; + ros::ServiceServer m_serviceDeleteAllSequenceTasks; + ros::ServiceServer m_serviceTakeSnapshot; + ros::ServiceServer m_serviceGetFirmwareBundleVersions; + ros::ServiceServer m_serviceMoveSequenceTask; + ros::ServiceServer m_serviceDuplicateMapping; + ros::ServiceServer m_serviceDuplicateMap; + ros::ServiceServer m_serviceSetControllerConfiguration; + ros::ServiceServer m_serviceGetControllerConfiguration; + ros::ServiceServer m_serviceGetAllControllerConfigurations; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h new file mode 100644 index 00000000..6475c3db --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h @@ -0,0 +1,142 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CONTROLCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_CONTROLCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/SetGravityVector.h" +#include "kortex_driver/GetGravityVector.h" +#include "kortex_driver/SetPayloadInformation.h" +#include "kortex_driver/GetPayloadInformation.h" +#include "kortex_driver/SetToolConfiguration.h" +#include "kortex_driver/GetToolConfiguration.h" +#include "kortex_driver/OnNotificationControlConfigurationTopic.h" +#include "kortex_driver/ControlConfigurationNotification.h" +#include "kortex_driver/ControlConfig_Unsubscribe.h" +#include "kortex_driver/SetCartesianReferenceFrame.h" +#include "kortex_driver/GetCartesianReferenceFrame.h" +#include "kortex_driver/ControlConfig_GetControlMode.h" +#include "kortex_driver/SetJointSpeedSoftLimits.h" +#include "kortex_driver/SetTwistLinearSoftLimit.h" +#include "kortex_driver/SetTwistAngularSoftLimit.h" +#include "kortex_driver/SetJointAccelerationSoftLimits.h" +#include "kortex_driver/GetKinematicHardLimits.h" +#include "kortex_driver/GetKinematicSoftLimits.h" +#include "kortex_driver/GetAllKinematicSoftLimits.h" +#include "kortex_driver/SetDesiredLinearTwist.h" +#include "kortex_driver/SetDesiredAngularTwist.h" +#include "kortex_driver/SetDesiredJointSpeeds.h" +#include "kortex_driver/GetDesiredSpeeds.h" +#include "kortex_driver/ResetGravityVector.h" +#include "kortex_driver/ResetPayloadInformation.h" +#include "kortex_driver/ResetToolConfiguration.h" +#include "kortex_driver/ResetJointSpeedSoftLimits.h" +#include "kortex_driver/ResetTwistLinearSoftLimit.h" +#include "kortex_driver/ResetTwistAngularSoftLimit.h" +#include "kortex_driver/ResetJointAccelerationSoftLimits.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IControlConfigServices +{ + public: + IControlConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) = 0; + virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) = 0; + virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) = 0; + virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) = 0; + virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) = 0; + virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) = 0; + virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) = 0; + virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) = 0; + virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) = 0; + virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) = 0; + virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) = 0; + virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) = 0; + virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) = 0; + virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) = 0; + virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) = 0; + virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) = 0; + virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) = 0; + virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) = 0; + virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) = 0; + virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) = 0; + virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) = 0; + virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) = 0; + virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) = 0; + virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) = 0; + virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) = 0; + virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) = 0; + virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) = 0; + virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) = 0; + virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) = 0; + virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_ControlConfigurationTopic; + bool m_is_activated_ControlConfigurationTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceSetGravityVector; + ros::ServiceServer m_serviceGetGravityVector; + ros::ServiceServer m_serviceSetPayloadInformation; + ros::ServiceServer m_serviceGetPayloadInformation; + ros::ServiceServer m_serviceSetToolConfiguration; + ros::ServiceServer m_serviceGetToolConfiguration; + ros::ServiceServer m_serviceOnNotificationControlConfigurationTopic; + ros::ServiceServer m_serviceControlConfig_Unsubscribe; + ros::ServiceServer m_serviceSetCartesianReferenceFrame; + ros::ServiceServer m_serviceGetCartesianReferenceFrame; + ros::ServiceServer m_serviceControlConfig_GetControlMode; + ros::ServiceServer m_serviceSetJointSpeedSoftLimits; + ros::ServiceServer m_serviceSetTwistLinearSoftLimit; + ros::ServiceServer m_serviceSetTwistAngularSoftLimit; + ros::ServiceServer m_serviceSetJointAccelerationSoftLimits; + ros::ServiceServer m_serviceGetKinematicHardLimits; + ros::ServiceServer m_serviceGetKinematicSoftLimits; + ros::ServiceServer m_serviceGetAllKinematicSoftLimits; + ros::ServiceServer m_serviceSetDesiredLinearTwist; + ros::ServiceServer m_serviceSetDesiredAngularTwist; + ros::ServiceServer m_serviceSetDesiredJointSpeeds; + ros::ServiceServer m_serviceGetDesiredSpeeds; + ros::ServiceServer m_serviceResetGravityVector; + ros::ServiceServer m_serviceResetPayloadInformation; + ros::ServiceServer m_serviceResetToolConfiguration; + ros::ServiceServer m_serviceResetJointSpeedSoftLimits; + ros::ServiceServer m_serviceResetTwistLinearSoftLimit; + ros::ServiceServer m_serviceResetTwistAngularSoftLimit; + ros::ServiceServer m_serviceResetJointAccelerationSoftLimits; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h new file mode 100644 index 00000000..4f4b7d1e --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h @@ -0,0 +1,151 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICECONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_DEVICECONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/GetRunMode.h" +#include "kortex_driver/SetRunMode.h" +#include "kortex_driver/GetDeviceType.h" +#include "kortex_driver/GetFirmwareVersion.h" +#include "kortex_driver/GetBootloaderVersion.h" +#include "kortex_driver/GetModelNumber.h" +#include "kortex_driver/GetPartNumber.h" +#include "kortex_driver/GetSerialNumber.h" +#include "kortex_driver/GetMACAddress.h" +#include "kortex_driver/GetIPv4Settings.h" +#include "kortex_driver/SetIPv4Settings.h" +#include "kortex_driver/GetPartNumberRevision.h" +#include "kortex_driver/RebootRequest.h" +#include "kortex_driver/SetSafetyEnable.h" +#include "kortex_driver/SetSafetyErrorThreshold.h" +#include "kortex_driver/SetSafetyWarningThreshold.h" +#include "kortex_driver/SetSafetyConfiguration.h" +#include "kortex_driver/GetSafetyConfiguration.h" +#include "kortex_driver/GetSafetyInformation.h" +#include "kortex_driver/GetSafetyEnable.h" +#include "kortex_driver/GetSafetyStatus.h" +#include "kortex_driver/ClearAllSafetyStatus.h" +#include "kortex_driver/ClearSafetyStatus.h" +#include "kortex_driver/GetAllSafetyConfiguration.h" +#include "kortex_driver/GetAllSafetyInformation.h" +#include "kortex_driver/ResetSafetyDefaults.h" +#include "kortex_driver/OnNotificationSafetyTopic.h" +#include "kortex_driver/SafetyNotification.h" +#include "kortex_driver/ExecuteCalibration.h" +#include "kortex_driver/GetCalibrationResult.h" +#include "kortex_driver/StopCalibration.h" +#include "kortex_driver/DeviceConfig_SetCapSenseConfig.h" +#include "kortex_driver/DeviceConfig_GetCapSenseConfig.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IDeviceConfigServices +{ + public: + IDeviceConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) = 0; + virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) = 0; + virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) = 0; + virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) = 0; + virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) = 0; + virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) = 0; + virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) = 0; + virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) = 0; + virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) = 0; + virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) = 0; + virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) = 0; + virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) = 0; + virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) = 0; + virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) = 0; + virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) = 0; + virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) = 0; + virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) = 0; + virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) = 0; + virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) = 0; + virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) = 0; + virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) = 0; + virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) = 0; + virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) = 0; + virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) = 0; + virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) = 0; + virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) = 0; + virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) = 0; + virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) = 0; + virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) = 0; + virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) = 0; + virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) = 0; + virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) = 0; + virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_SafetyTopic; + bool m_is_activated_SafetyTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceGetRunMode; + ros::ServiceServer m_serviceSetRunMode; + ros::ServiceServer m_serviceGetDeviceType; + ros::ServiceServer m_serviceGetFirmwareVersion; + ros::ServiceServer m_serviceGetBootloaderVersion; + ros::ServiceServer m_serviceGetModelNumber; + ros::ServiceServer m_serviceGetPartNumber; + ros::ServiceServer m_serviceGetSerialNumber; + ros::ServiceServer m_serviceGetMACAddress; + ros::ServiceServer m_serviceGetIPv4Settings; + ros::ServiceServer m_serviceSetIPv4Settings; + ros::ServiceServer m_serviceGetPartNumberRevision; + ros::ServiceServer m_serviceRebootRequest; + ros::ServiceServer m_serviceSetSafetyEnable; + ros::ServiceServer m_serviceSetSafetyErrorThreshold; + ros::ServiceServer m_serviceSetSafetyWarningThreshold; + ros::ServiceServer m_serviceSetSafetyConfiguration; + ros::ServiceServer m_serviceGetSafetyConfiguration; + ros::ServiceServer m_serviceGetSafetyInformation; + ros::ServiceServer m_serviceGetSafetyEnable; + ros::ServiceServer m_serviceGetSafetyStatus; + ros::ServiceServer m_serviceClearAllSafetyStatus; + ros::ServiceServer m_serviceClearSafetyStatus; + ros::ServiceServer m_serviceGetAllSafetyConfiguration; + ros::ServiceServer m_serviceGetAllSafetyInformation; + ros::ServiceServer m_serviceResetSafetyDefaults; + ros::ServiceServer m_serviceOnNotificationSafetyTopic; + ros::ServiceServer m_serviceExecuteCalibration; + ros::ServiceServer m_serviceGetCalibrationResult; + ros::ServiceServer m_serviceStopCalibration; + ros::ServiceServer m_serviceDeviceConfig_SetCapSenseConfig; + ros::ServiceServer m_serviceDeviceConfig_GetCapSenseConfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h new file mode 100644 index 00000000..a2e544ed --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h @@ -0,0 +1,54 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_SERVICES_INTERFACE_H_ +#define _KORTEX_DEVICEMANAGER_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/ReadAllDevices.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IDeviceManagerServices +{ + public: + IDeviceManagerServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceReadAllDevices; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h new file mode 100644 index 00000000..dc370d10 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h @@ -0,0 +1,93 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_INTERCONNECTCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_INTERCONNECTCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/GetUARTConfiguration.h" +#include "kortex_driver/SetUARTConfiguration.h" +#include "kortex_driver/GetEthernetConfiguration.h" +#include "kortex_driver/SetEthernetConfiguration.h" +#include "kortex_driver/GetGPIOConfiguration.h" +#include "kortex_driver/SetGPIOConfiguration.h" +#include "kortex_driver/GetGPIOState.h" +#include "kortex_driver/SetGPIOState.h" +#include "kortex_driver/GetI2CConfiguration.h" +#include "kortex_driver/SetI2CConfiguration.h" +#include "kortex_driver/I2CRead.h" +#include "kortex_driver/I2CReadRegister.h" +#include "kortex_driver/I2CWrite.h" +#include "kortex_driver/I2CWriteRegister.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IInterconnectConfigServices +{ + public: + IInterconnectConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) = 0; + virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) = 0; + virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) = 0; + virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) = 0; + virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) = 0; + virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) = 0; + virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) = 0; + virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) = 0; + virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) = 0; + virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) = 0; + virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) = 0; + virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) = 0; + virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) = 0; + virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceGetUARTConfiguration; + ros::ServiceServer m_serviceSetUARTConfiguration; + ros::ServiceServer m_serviceGetEthernetConfiguration; + ros::ServiceServer m_serviceSetEthernetConfiguration; + ros::ServiceServer m_serviceGetGPIOConfiguration; + ros::ServiceServer m_serviceSetGPIOConfiguration; + ros::ServiceServer m_serviceGetGPIOState; + ros::ServiceServer m_serviceSetGPIOState; + ros::ServiceServer m_serviceGetI2CConfiguration; + ros::ServiceServer m_serviceSetI2CConfiguration; + ros::ServiceServer m_serviceI2CRead; + ros::ServiceServer m_serviceI2CReadRegister; + ros::ServiceServer m_serviceI2CWrite; + ros::ServiceServer m_serviceI2CWriteRegister; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h new file mode 100644 index 00000000..1bf98c7d --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h @@ -0,0 +1,91 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VISIONCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_VISIONCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/SetSensorSettings.h" +#include "kortex_driver/GetSensorSettings.h" +#include "kortex_driver/GetOptionValue.h" +#include "kortex_driver/SetOptionValue.h" +#include "kortex_driver/GetOptionInformation.h" +#include "kortex_driver/OnNotificationVisionTopic.h" +#include "kortex_driver/VisionNotification.h" +#include "kortex_driver/DoSensorFocusAction.h" +#include "kortex_driver/GetIntrinsicParameters.h" +#include "kortex_driver/GetIntrinsicParametersProfile.h" +#include "kortex_driver/SetIntrinsicParameters.h" +#include "kortex_driver/GetExtrinsicParameters.h" +#include "kortex_driver/SetExtrinsicParameters.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IVisionConfigServices +{ + public: + IVisionConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) = 0; + virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) = 0; + virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) = 0; + virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) = 0; + virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) = 0; + virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) = 0; + virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) = 0; + virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) = 0; + virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) = 0; + virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) = 0; + virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) = 0; + virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) = 0; + virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_VisionTopic; + bool m_is_activated_VisionTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceSetSensorSettings; + ros::ServiceServer m_serviceGetSensorSettings; + ros::ServiceServer m_serviceGetOptionValue; + ros::ServiceServer m_serviceSetOptionValue; + ros::ServiceServer m_serviceGetOptionInformation; + ros::ServiceServer m_serviceOnNotificationVisionTopic; + ros::ServiceServer m_serviceDoSensorFocusAction; + ros::ServiceServer m_serviceGetIntrinsicParameters; + ros::ServiceServer m_serviceGetIntrinsicParametersProfile; + ros::ServiceServer m_serviceSetIntrinsicParameters; + ros::ServiceServer m_serviceGetExtrinsicParameters; + ros::ServiceServer m_serviceSetExtrinsicParameters; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/actuatorconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_proto_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/actuatorconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_proto_converter.h index b5dd37e9..e91312bc 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/AxisPosition.h" diff --git a/kortex_driver/include/kortex_driver/generated/actuatorconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_ros_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/actuatorconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_ros_converter.h index 3412b504..024e75af 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/AxisPosition.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h new file mode 100644 index 00000000..0a7adacf --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h @@ -0,0 +1,61 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ACTUATORCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_ACTUATORCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/actuatorconfig_services_interface.h" + +#include +#include + +using namespace std; + +class ActuatorConfigRobotServices : public IActuatorConfigServices +{ + public: + ActuatorConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) override; + virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) override; + virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) override; + virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) override; + virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) override; + virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) override; + virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) override; + virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) override; + virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) override; + virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) override; + virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) override; + virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) override; + virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) override; + virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) override; + virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) override; + virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) override; + virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) override; + virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) override; + virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) override; + virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::ActuatorConfig::ActuatorConfigClient* m_actuatorconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_proto_converter.h similarity index 59% rename from kortex_driver/include/kortex_driver/generated/actuatorcyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_proto_converter.h index cff91c67..f463e1bc 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/ActuatorCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_ros_converter.h similarity index 59% rename from kortex_driver/include/kortex_driver/generated/actuatorcyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_ros_converter.h index 62be5681..02085e5b 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/ActuatorCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/base_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h similarity index 96% rename from kortex_driver/include/kortex_driver/generated/base_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h index 52a4a7a5..23cc90cf 100644 --- a/kortex_driver/include/kortex_driver/generated/base_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/FullUserProfile.h" diff --git a/kortex_driver/include/kortex_driver/generated/base_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h similarity index 96% rename from kortex_driver/include/kortex_driver/generated/base_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h index c05cd620..0cb57ed1 100644 --- a/kortex_driver/include/kortex_driver/generated/base_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/FullUserProfile.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/base_services.h b/kortex_driver/include/kortex_driver/generated/robot/base_services.h new file mode 100644 index 00000000..27c48320 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/base_services.h @@ -0,0 +1,199 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BASE_ROBOT_SERVICES_H_ +#define _KORTEX_BASE_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/base_services_interface.h" + +#include +#include + +using namespace std; + +class BaseRobotServices : public IBaseServices +{ + public: + BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) override; + virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) override; + virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) override; + virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) override; + virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) override; + virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) override; + virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) override; + virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) override; + virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) override; + virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) override; + virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) override; + virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) override; + virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) override; + virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) override; + virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) override; + virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) override; + virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) override; + virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) override; + virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) override; + virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) override; + virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) override; + virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) override; + virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) override; + virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) override; + virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) override; + virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) override; + virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) override; + virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) override; + virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) override; + virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) override; + virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) override; + virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) override; + virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) override; + virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) override; + virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) override; + virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) override; + virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) override; + virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) override; + virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) override; + virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) override; + virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) override; + virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) override; + virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) override; + virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) override; + virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) override; + virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) override; + virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) override; + virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) override; + virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) override; + virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) override; + virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) override; + virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) override; + virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) override; + virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) override; + virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) override; + virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) override; + virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) override; + virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; + virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; + virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; + virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; + virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; + virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; + virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; + virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) override; + virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) override; + virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) override; + virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) override; + virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) override; + virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) override; + virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) override; + virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) override; + virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) override; + virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) override; + virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) override; + virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) override; + virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) override; + virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) override; + virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) override; + virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) override; + virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) override; + virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) override; + virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) override; + virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) override; + virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) override; + virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) override; + virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) override; + virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) override; + virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) override; + virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) override; + virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) override; + virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) override; + virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) override; + virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) override; + virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) override; + virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) override; + virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) override; + virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) override; + virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) override; + virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) override; + virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) override; + virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) override; + virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) override; + virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) override; + virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) override; + virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) override; + virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) override; + virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) override; + virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) override; + virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) override; + virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) override; + virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) override; + virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) override; + virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) override; + virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) override; + virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) override; + virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) override; + virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) override; + virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) override; + virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) override; + virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) override; + virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) override; + virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) override; + virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) override; + virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) override; + virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) override; + virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) override; + virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) override; + virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) override; + virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) override; + virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) override; + virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) override; + virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) override; + virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) override; + virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) override; + virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) override; + virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) override; + virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) override; + virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) override; + virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) override; + virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) override; + virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) override; + virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) override; + virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) override; + virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) override; + virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) override; + virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) override; + virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) override; + virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) override; + virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; + virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; + virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; + virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; + virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; + virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; + virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; + virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; + virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::Base::BaseClient* m_base; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/basecyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_proto_converter.h similarity index 65% rename from kortex_driver/include/kortex_driver/generated/basecyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/basecyclic_proto_converter.h index 7d5b660e..ba500e29 100644 --- a/kortex_driver/include/kortex_driver/generated/basecyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/ActuatorCommand.h" diff --git a/kortex_driver/include/kortex_driver/generated/basecyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_ros_converter.h similarity index 65% rename from kortex_driver/include/kortex_driver/generated/basecyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/basecyclic_ros_converter.h index dde72627..d0f8a8a6 100644 --- a/kortex_driver/include/kortex_driver/generated/basecyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/ActuatorCommand.h" diff --git a/kortex_driver/include/kortex_driver/generated/common_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/common_proto_converter.h similarity index 72% rename from kortex_driver/include/kortex_driver/generated/common_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/common_proto_converter.h index 8fef504b..786db33b 100644 --- a/kortex_driver/include/kortex_driver/generated/common_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/common_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/DeviceHandle.h" diff --git a/kortex_driver/include/kortex_driver/generated/common_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/common_ros_converter.h similarity index 72% rename from kortex_driver/include/kortex_driver/generated/common_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/common_ros_converter.h index 0f1147b6..6aa14ec6 100644 --- a/kortex_driver/include/kortex_driver/generated/common_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/common_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/DeviceHandle.h" diff --git a/kortex_driver/include/kortex_driver/generated/controlconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/controlconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h index c93da6a3..6b19dd05 100644 --- a/kortex_driver/include/kortex_driver/generated/controlconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/GravityVector.h" diff --git a/kortex_driver/include/kortex_driver/generated/controlconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/controlconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h index c22531fb..6fd389fb 100644 --- a/kortex_driver/include/kortex_driver/generated/controlconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/GravityVector.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h new file mode 100644 index 00000000..e4adde31 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h @@ -0,0 +1,71 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CONTROLCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_CONTROLCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/controlconfig_services_interface.h" + +#include +#include + +using namespace std; + +class ControlConfigRobotServices : public IControlConfigServices +{ + public: + ControlConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) override; + virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) override; + virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) override; + virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) override; + virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) override; + virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) override; + virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) override; + virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) override; + virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) override; + virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) override; + virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) override; + virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) override; + virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) override; + virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) override; + virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) override; + virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) override; + virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) override; + virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) override; + virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) override; + virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) override; + virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) override; + virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) override; + virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) override; + virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) override; + virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) override; + virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) override; + virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) override; + virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; + virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; + virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::ControlConfig::ControlConfigClient* m_controlconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/deviceconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_proto_converter.h similarity index 83% rename from kortex_driver/include/kortex_driver/generated/deviceconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/deviceconfig_proto_converter.h index 38e025c7..67448259 100644 --- a/kortex_driver/include/kortex_driver/generated/deviceconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/DeviceType.h" diff --git a/kortex_driver/include/kortex_driver/generated/deviceconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_ros_converter.h similarity index 83% rename from kortex_driver/include/kortex_driver/generated/deviceconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/deviceconfig_ros_converter.h index 4e727c19..9dbe819a 100644 --- a/kortex_driver/include/kortex_driver/generated/deviceconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/DeviceType.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h new file mode 100644 index 00000000..fb87a132 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h @@ -0,0 +1,74 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICECONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_DEVICECONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/deviceconfig_services_interface.h" + +#include +#include + +using namespace std; + +class DeviceConfigRobotServices : public IDeviceConfigServices +{ + public: + DeviceConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) override; + virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) override; + virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) override; + virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) override; + virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) override; + virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) override; + virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) override; + virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) override; + virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) override; + virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) override; + virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) override; + virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) override; + virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) override; + virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) override; + virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) override; + virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) override; + virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) override; + virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) override; + virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) override; + virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) override; + virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) override; + virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) override; + virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) override; + virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) override; + virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) override; + virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) override; + virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) override; + virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) override; + virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) override; + virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) override; + virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) override; + virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) override; + virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::DeviceConfig::DeviceConfigClient* m_deviceconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h new file mode 100644 index 00000000..42e63e7e --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h @@ -0,0 +1,49 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ +#define _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" + + +#include "kortex_driver/DeviceHandles.h" + + +int ToProtoData(kortex_driver::DeviceHandles input, Kinova::Api::DeviceManager::DeviceHandles *output); + +#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h new file mode 100644 index 00000000..3847a80b --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h @@ -0,0 +1,49 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ +#define _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include + +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" + + +#include "kortex_driver/DeviceHandles.h" + + +int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::DeviceHandles &output); + +#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h new file mode 100644 index 00000000..26443eca --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h @@ -0,0 +1,42 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_ROBOT_SERVICES_H_ +#define _KORTEX_DEVICEMANAGER_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/devicemanager_services_interface.h" + +#include +#include + +using namespace std; + +class DeviceManagerRobotServices : public IDeviceManagerServices +{ + public: + DeviceManagerRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::DeviceManager::DeviceManagerClient* m_devicemanager; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/grippercyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_proto_converter.h similarity index 66% rename from kortex_driver/include/kortex_driver/generated/grippercyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/grippercyclic_proto_converter.h index fc5faad4..9a696f5f 100644 --- a/kortex_driver/include/kortex_driver/generated/grippercyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/GripperCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/grippercyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_ros_converter.h similarity index 66% rename from kortex_driver/include/kortex_driver/generated/grippercyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/grippercyclic_ros_converter.h index e352379a..a584b91e 100644 --- a/kortex_driver/include/kortex_driver/generated/grippercyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/GripperCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/interconnectconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h similarity index 75% rename from kortex_driver/include/kortex_driver/generated/interconnectconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h index 0f3d45cc..4b201c00 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/EthernetDeviceIdentification.h" diff --git a/kortex_driver/include/kortex_driver/generated/interconnectconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h similarity index 75% rename from kortex_driver/include/kortex_driver/generated/interconnectconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h index 0f6187c3..f155cf65 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/EthernetDeviceIdentification.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h new file mode 100644 index 00000000..a4f360eb --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h @@ -0,0 +1,55 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_INTERCONNECTCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_INTERCONNECTCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/interconnectconfig_services_interface.h" + +#include +#include + +using namespace std; + +class InterconnectConfigRobotServices : public IInterconnectConfigServices +{ + public: + InterconnectConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) override; + virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) override; + virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) override; + virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) override; + virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) override; + virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) override; + virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) override; + virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) override; + virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) override; + virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) override; + virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) override; + virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) override; + virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) override; + virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::InterconnectConfig::InterconnectConfigClient* m_interconnectconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_proto_converter.h similarity index 60% rename from kortex_driver/include/kortex_driver/generated/interconnectcyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_proto_converter.h index ba34f394..ad838be9 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/InterconnectCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_ros_converter.h similarity index 60% rename from kortex_driver/include/kortex_driver/generated/interconnectcyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_ros_converter.h index 2e89a38c..eefda5c7 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/InterconnectCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/productconfiguration_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_proto_converter.h similarity index 54% rename from kortex_driver/include/kortex_driver/generated/productconfiguration_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/productconfiguration_proto_converter.h index 213530f8..c40b896b 100644 --- a/kortex_driver/include/kortex_driver/generated/productconfiguration_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/CompleteProductConfiguration.h" diff --git a/kortex_driver/include/kortex_driver/generated/productconfiguration_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_ros_converter.h similarity index 54% rename from kortex_driver/include/kortex_driver/generated/productconfiguration_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/productconfiguration_ros_converter.h index 4998b0a2..757cc526 100644 --- a/kortex_driver/include/kortex_driver/generated/productconfiguration_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/CompleteProductConfiguration.h" diff --git a/kortex_driver/include/kortex_driver/generated/visionconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_proto_converter.h similarity index 78% rename from kortex_driver/include/kortex_driver/generated/visionconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/visionconfig_proto_converter.h index 2b643df0..ed757cda 100644 --- a/kortex_driver/include/kortex_driver/generated/visionconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" #include "kortex_driver/SensorSettings.h" diff --git a/kortex_driver/include/kortex_driver/generated/visionconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_ros_converter.h similarity index 78% rename from kortex_driver/include/kortex_driver/generated/visionconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/visionconfig_ros_converter.h index 74cca282..3d4a961f 100644 --- a/kortex_driver/include/kortex_driver/generated/visionconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" #include "kortex_driver/SensorSettings.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h new file mode 100644 index 00000000..32e0129a --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h @@ -0,0 +1,54 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VISIONCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_VISIONCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/visionconfig_services_interface.h" + +#include +#include + +using namespace std; + +class VisionConfigRobotServices : public IVisionConfigServices +{ + public: + VisionConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) override; + virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) override; + virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) override; + virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) override; + virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) override; + virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) override; + virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) override; + virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) override; + virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) override; + virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) override; + virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) override; + virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) override; + virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::VisionConfig::VisionConfigClient* m_visionconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h new file mode 100644 index 00000000..0a04e424 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h @@ -0,0 +1,73 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ACTUATORCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_ACTUATORCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/actuatorconfig_services_interface.h" + +using namespace std; + +class ActuatorConfigSimulationServices : public IActuatorConfigServices +{ + public: + ActuatorConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function GetAxisOffsetsHandler = nullptr; + virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) override; + std::function SetAxisOffsetsHandler = nullptr; + virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) override; + std::function SetTorqueOffsetHandler = nullptr; + virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) override; + std::function ActuatorConfig_GetControlModeHandler = nullptr; + virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) override; + std::function SetControlModeHandler = nullptr; + virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) override; + std::function GetActivatedControlLoopHandler = nullptr; + virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) override; + std::function SetActivatedControlLoopHandler = nullptr; + virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) override; + std::function GetControlLoopParametersHandler = nullptr; + virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) override; + std::function SetControlLoopParametersHandler = nullptr; + virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) override; + std::function SelectCustomDataHandler = nullptr; + virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) override; + std::function GetSelectedCustomDataHandler = nullptr; + virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) override; + std::function SetCommandModeHandler = nullptr; + virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) override; + std::function ActuatorConfig_ClearFaultsHandler = nullptr; + virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) override; + std::function SetServoingHandler = nullptr; + virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) override; + std::function MoveToPositionHandler = nullptr; + virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) override; + std::function GetCommandModeHandler = nullptr; + virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) override; + std::function GetServoingHandler = nullptr; + virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) override; + std::function GetTorqueOffsetHandler = nullptr; + virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) override; + std::function SetCoggingFeedforwardModeHandler = nullptr; + virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) override; + std::function GetCoggingFeedforwardModeHandler = nullptr; + virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h new file mode 100644 index 00000000..65566751 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h @@ -0,0 +1,335 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BASE_SIMULATION_SERVICES_H_ +#define _KORTEX_BASE_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/base_services_interface.h" + +using namespace std; + +class BaseSimulationServices : public IBaseServices +{ + public: + BaseSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function CreateUserProfileHandler = nullptr; + virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) override; + std::function UpdateUserProfileHandler = nullptr; + virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) override; + std::function ReadUserProfileHandler = nullptr; + virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) override; + std::function DeleteUserProfileHandler = nullptr; + virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) override; + std::function ReadAllUserProfilesHandler = nullptr; + virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) override; + std::function ReadAllUsersHandler = nullptr; + virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) override; + std::function ChangePasswordHandler = nullptr; + virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) override; + std::function CreateSequenceHandler = nullptr; + virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) override; + std::function UpdateSequenceHandler = nullptr; + virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) override; + std::function ReadSequenceHandler = nullptr; + virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) override; + std::function DeleteSequenceHandler = nullptr; + virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) override; + std::function ReadAllSequencesHandler = nullptr; + virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) override; + std::function PlaySequenceHandler = nullptr; + virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) override; + std::function PlayAdvancedSequenceHandler = nullptr; + virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) override; + std::function StopSequenceHandler = nullptr; + virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) override; + std::function PauseSequenceHandler = nullptr; + virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) override; + std::function ResumeSequenceHandler = nullptr; + virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) override; + std::function CreateProtectionZoneHandler = nullptr; + virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) override; + std::function UpdateProtectionZoneHandler = nullptr; + virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) override; + std::function ReadProtectionZoneHandler = nullptr; + virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) override; + std::function DeleteProtectionZoneHandler = nullptr; + virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) override; + std::function ReadAllProtectionZonesHandler = nullptr; + virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) override; + std::function CreateMappingHandler = nullptr; + virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) override; + std::function ReadMappingHandler = nullptr; + virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) override; + std::function UpdateMappingHandler = nullptr; + virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) override; + std::function DeleteMappingHandler = nullptr; + virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) override; + std::function ReadAllMappingsHandler = nullptr; + virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) override; + std::function CreateMapHandler = nullptr; + virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) override; + std::function ReadMapHandler = nullptr; + virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) override; + std::function UpdateMapHandler = nullptr; + virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) override; + std::function DeleteMapHandler = nullptr; + virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) override; + std::function ReadAllMapsHandler = nullptr; + virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) override; + std::function ActivateMapHandler = nullptr; + virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) override; + std::function CreateActionHandler = nullptr; + virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) override; + std::function ReadActionHandler = nullptr; + virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) override; + std::function ReadAllActionsHandler = nullptr; + virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) override; + std::function DeleteActionHandler = nullptr; + virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) override; + std::function UpdateActionHandler = nullptr; + virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) override; + std::function ExecuteActionFromReferenceHandler = nullptr; + virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) override; + std::function ExecuteActionHandler = nullptr; + virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) override; + std::function PauseActionHandler = nullptr; + virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) override; + std::function StopActionHandler = nullptr; + virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) override; + std::function ResumeActionHandler = nullptr; + virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) override; + std::function GetIPv4ConfigurationHandler = nullptr; + virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) override; + std::function SetIPv4ConfigurationHandler = nullptr; + virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) override; + std::function SetCommunicationInterfaceEnableHandler = nullptr; + virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) override; + std::function IsCommunicationInterfaceEnableHandler = nullptr; + virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) override; + std::function GetAvailableWifiHandler = nullptr; + virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) override; + std::function GetWifiInformationHandler = nullptr; + virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) override; + std::function AddWifiConfigurationHandler = nullptr; + virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) override; + std::function DeleteWifiConfigurationHandler = nullptr; + virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) override; + std::function GetAllConfiguredWifisHandler = nullptr; + virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) override; + std::function ConnectWifiHandler = nullptr; + virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) override; + std::function DisconnectWifiHandler = nullptr; + virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) override; + std::function GetConnectedWifiInformationHandler = nullptr; + virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) override; + std::function Base_UnsubscribeHandler = nullptr; + virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) override; + std::function OnNotificationConfigurationChangeTopicHandler = nullptr; + virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) override; + virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; + std::function OnNotificationMappingInfoTopicHandler = nullptr; + virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; + virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; + std::function OnNotificationControlModeTopicHandler = nullptr; + virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; + virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; + std::function OnNotificationOperatingModeTopicHandler = nullptr; + virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; + virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; + std::function OnNotificationSequenceInfoTopicHandler = nullptr; + virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) override; + virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) override; + std::function OnNotificationProtectionZoneTopicHandler = nullptr; + virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) override; + virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) override; + std::function OnNotificationUserTopicHandler = nullptr; + virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) override; + virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) override; + std::function OnNotificationControllerTopicHandler = nullptr; + virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) override; + virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) override; + std::function OnNotificationActionTopicHandler = nullptr; + virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) override; + virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) override; + std::function OnNotificationRobotEventTopicHandler = nullptr; + virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) override; + virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) override; + std::function PlayCartesianTrajectoryHandler = nullptr; + virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) override; + std::function PlayCartesianTrajectoryPositionHandler = nullptr; + virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) override; + std::function PlayCartesianTrajectoryOrientationHandler = nullptr; + virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) override; + std::function StopHandler = nullptr; + virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) override; + std::function GetMeasuredCartesianPoseHandler = nullptr; + virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) override; + std::function SendWrenchCommandHandler = nullptr; + virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) override; + std::function SendWrenchJoystickCommandHandler = nullptr; + virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) override; + std::function SendTwistJoystickCommandHandler = nullptr; + virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) override; + std::function SendTwistCommandHandler = nullptr; + virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) override; + std::function PlayJointTrajectoryHandler = nullptr; + virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) override; + std::function PlaySelectedJointTrajectoryHandler = nullptr; + virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) override; + std::function GetMeasuredJointAnglesHandler = nullptr; + virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) override; + std::function SendJointSpeedsCommandHandler = nullptr; + virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) override; + std::function SendSelectedJointSpeedCommandHandler = nullptr; + virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) override; + std::function SendGripperCommandHandler = nullptr; + virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) override; + std::function GetMeasuredGripperMovementHandler = nullptr; + virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) override; + std::function SetAdmittanceHandler = nullptr; + virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) override; + std::function SetOperatingModeHandler = nullptr; + virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) override; + std::function ApplyEmergencyStopHandler = nullptr; + virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) override; + std::function Base_ClearFaultsHandler = nullptr; + virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) override; + std::function Base_GetControlModeHandler = nullptr; + virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) override; + std::function GetOperatingModeHandler = nullptr; + virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) override; + std::function SetServoingModeHandler = nullptr; + virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) override; + std::function GetServoingModeHandler = nullptr; + virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) override; + std::function OnNotificationServoingModeTopicHandler = nullptr; + virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) override; + virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) override; + std::function RestoreFactorySettingsHandler = nullptr; + virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) override; + std::function OnNotificationFactoryTopicHandler = nullptr; + virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) override; + virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) override; + std::function GetAllConnectedControllersHandler = nullptr; + virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) override; + std::function GetControllerStateHandler = nullptr; + virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) override; + std::function GetActuatorCountHandler = nullptr; + virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) override; + std::function StartWifiScanHandler = nullptr; + virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) override; + std::function GetConfiguredWifiHandler = nullptr; + virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) override; + std::function OnNotificationNetworkTopicHandler = nullptr; + virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) override; + virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) override; + std::function GetArmStateHandler = nullptr; + virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) override; + std::function OnNotificationArmStateTopicHandler = nullptr; + virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) override; + virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) override; + std::function GetIPv4InformationHandler = nullptr; + virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) override; + std::function SetWifiCountryCodeHandler = nullptr; + virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) override; + std::function GetWifiCountryCodeHandler = nullptr; + virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) override; + std::function Base_SetCapSenseConfigHandler = nullptr; + virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) override; + std::function Base_GetCapSenseConfigHandler = nullptr; + virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) override; + std::function GetAllJointsSpeedHardLimitationHandler = nullptr; + virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) override; + std::function GetAllJointsTorqueHardLimitationHandler = nullptr; + virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) override; + std::function GetTwistHardLimitationHandler = nullptr; + virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) override; + std::function GetWrenchHardLimitationHandler = nullptr; + virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) override; + std::function SendJointSpeedsJoystickCommandHandler = nullptr; + virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) override; + std::function SendSelectedJointSpeedJoystickCommandHandler = nullptr; + virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) override; + std::function EnableBridgeHandler = nullptr; + virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) override; + std::function DisableBridgeHandler = nullptr; + virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) override; + std::function GetBridgeListHandler = nullptr; + virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) override; + std::function GetBridgeConfigHandler = nullptr; + virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) override; + std::function PlayPreComputedJointTrajectoryHandler = nullptr; + virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) override; + std::function GetProductConfigurationHandler = nullptr; + virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) override; + std::function UpdateEndEffectorTypeConfigurationHandler = nullptr; + virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) override; + std::function RestoreFactoryProductConfigurationHandler = nullptr; + virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) override; + std::function GetTrajectoryErrorReportHandler = nullptr; + virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) override; + std::function GetAllJointsSpeedSoftLimitationHandler = nullptr; + virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) override; + std::function GetAllJointsTorqueSoftLimitationHandler = nullptr; + virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) override; + std::function GetTwistSoftLimitationHandler = nullptr; + virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) override; + std::function GetWrenchSoftLimitationHandler = nullptr; + virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) override; + std::function SetControllerConfigurationModeHandler = nullptr; + virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) override; + std::function GetControllerConfigurationModeHandler = nullptr; + virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) override; + std::function StartTeachingHandler = nullptr; + virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) override; + std::function StopTeachingHandler = nullptr; + virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) override; + std::function AddSequenceTasksHandler = nullptr; + virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) override; + std::function UpdateSequenceTaskHandler = nullptr; + virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) override; + std::function SwapSequenceTasksHandler = nullptr; + virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) override; + std::function ReadSequenceTaskHandler = nullptr; + virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) override; + std::function ReadAllSequenceTasksHandler = nullptr; + virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) override; + std::function DeleteSequenceTaskHandler = nullptr; + virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) override; + std::function DeleteAllSequenceTasksHandler = nullptr; + virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; + std::function TakeSnapshotHandler = nullptr; + virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; + std::function GetFirmwareBundleVersionsHandler = nullptr; + virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; + std::function MoveSequenceTaskHandler = nullptr; + virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; + std::function DuplicateMappingHandler = nullptr; + virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; + std::function DuplicateMapHandler = nullptr; + virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; + std::function SetControllerConfigurationHandler = nullptr; + virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; + std::function GetControllerConfigurationHandler = nullptr; + virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; + std::function GetAllControllerConfigurationsHandler = nullptr; + virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h new file mode 100644 index 00000000..30a8738a --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h @@ -0,0 +1,92 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CONTROLCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_CONTROLCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/controlconfig_services_interface.h" + +using namespace std; + +class ControlConfigSimulationServices : public IControlConfigServices +{ + public: + ControlConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function SetGravityVectorHandler = nullptr; + virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) override; + std::function GetGravityVectorHandler = nullptr; + virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) override; + std::function SetPayloadInformationHandler = nullptr; + virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) override; + std::function GetPayloadInformationHandler = nullptr; + virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) override; + std::function SetToolConfigurationHandler = nullptr; + virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) override; + std::function GetToolConfigurationHandler = nullptr; + virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) override; + std::function OnNotificationControlConfigurationTopicHandler = nullptr; + virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) override; + virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) override; + std::function ControlConfig_UnsubscribeHandler = nullptr; + virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) override; + std::function SetCartesianReferenceFrameHandler = nullptr; + virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) override; + std::function GetCartesianReferenceFrameHandler = nullptr; + virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) override; + std::function ControlConfig_GetControlModeHandler = nullptr; + virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) override; + std::function SetJointSpeedSoftLimitsHandler = nullptr; + virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) override; + std::function SetTwistLinearSoftLimitHandler = nullptr; + virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) override; + std::function SetTwistAngularSoftLimitHandler = nullptr; + virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) override; + std::function SetJointAccelerationSoftLimitsHandler = nullptr; + virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) override; + std::function GetKinematicHardLimitsHandler = nullptr; + virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) override; + std::function GetKinematicSoftLimitsHandler = nullptr; + virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) override; + std::function GetAllKinematicSoftLimitsHandler = nullptr; + virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) override; + std::function SetDesiredLinearTwistHandler = nullptr; + virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) override; + std::function SetDesiredAngularTwistHandler = nullptr; + virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) override; + std::function SetDesiredJointSpeedsHandler = nullptr; + virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) override; + std::function GetDesiredSpeedsHandler = nullptr; + virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) override; + std::function ResetGravityVectorHandler = nullptr; + virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) override; + std::function ResetPayloadInformationHandler = nullptr; + virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) override; + std::function ResetToolConfigurationHandler = nullptr; + virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) override; + std::function ResetJointSpeedSoftLimitsHandler = nullptr; + virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) override; + std::function ResetTwistLinearSoftLimitHandler = nullptr; + virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; + std::function ResetTwistAngularSoftLimitHandler = nullptr; + virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; + std::function ResetJointAccelerationSoftLimitsHandler = nullptr; + virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h new file mode 100644 index 00000000..d047b456 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h @@ -0,0 +1,98 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICECONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_DEVICECONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/deviceconfig_services_interface.h" + +using namespace std; + +class DeviceConfigSimulationServices : public IDeviceConfigServices +{ + public: + DeviceConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function GetRunModeHandler = nullptr; + virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) override; + std::function SetRunModeHandler = nullptr; + virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) override; + std::function GetDeviceTypeHandler = nullptr; + virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) override; + std::function GetFirmwareVersionHandler = nullptr; + virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) override; + std::function GetBootloaderVersionHandler = nullptr; + virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) override; + std::function GetModelNumberHandler = nullptr; + virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) override; + std::function GetPartNumberHandler = nullptr; + virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) override; + std::function GetSerialNumberHandler = nullptr; + virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) override; + std::function GetMACAddressHandler = nullptr; + virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) override; + std::function GetIPv4SettingsHandler = nullptr; + virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) override; + std::function SetIPv4SettingsHandler = nullptr; + virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) override; + std::function GetPartNumberRevisionHandler = nullptr; + virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) override; + std::function RebootRequestHandler = nullptr; + virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) override; + std::function SetSafetyEnableHandler = nullptr; + virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) override; + std::function SetSafetyErrorThresholdHandler = nullptr; + virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) override; + std::function SetSafetyWarningThresholdHandler = nullptr; + virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) override; + std::function SetSafetyConfigurationHandler = nullptr; + virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) override; + std::function GetSafetyConfigurationHandler = nullptr; + virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) override; + std::function GetSafetyInformationHandler = nullptr; + virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) override; + std::function GetSafetyEnableHandler = nullptr; + virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) override; + std::function GetSafetyStatusHandler = nullptr; + virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) override; + std::function ClearAllSafetyStatusHandler = nullptr; + virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) override; + std::function ClearSafetyStatusHandler = nullptr; + virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) override; + std::function GetAllSafetyConfigurationHandler = nullptr; + virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) override; + std::function GetAllSafetyInformationHandler = nullptr; + virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) override; + std::function ResetSafetyDefaultsHandler = nullptr; + virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) override; + std::function OnNotificationSafetyTopicHandler = nullptr; + virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) override; + virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) override; + std::function ExecuteCalibrationHandler = nullptr; + virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) override; + std::function GetCalibrationResultHandler = nullptr; + virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) override; + std::function StopCalibrationHandler = nullptr; + virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) override; + std::function DeviceConfig_SetCapSenseConfigHandler = nullptr; + virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) override; + std::function DeviceConfig_GetCapSenseConfigHandler = nullptr; + virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h new file mode 100644 index 00000000..0c45ae75 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h @@ -0,0 +1,35 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_SIMULATION_SERVICES_H_ +#define _KORTEX_DEVICEMANAGER_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/devicemanager_services_interface.h" + +using namespace std; + +class DeviceManagerSimulationServices : public IDeviceManagerServices +{ + public: + DeviceManagerSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function ReadAllDevicesHandler = nullptr; + virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h new file mode 100644 index 00000000..1053941f --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h @@ -0,0 +1,61 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_INTERCONNECTCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_INTERCONNECTCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/interconnectconfig_services_interface.h" + +using namespace std; + +class InterconnectConfigSimulationServices : public IInterconnectConfigServices +{ + public: + InterconnectConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function GetUARTConfigurationHandler = nullptr; + virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) override; + std::function SetUARTConfigurationHandler = nullptr; + virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) override; + std::function GetEthernetConfigurationHandler = nullptr; + virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) override; + std::function SetEthernetConfigurationHandler = nullptr; + virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) override; + std::function GetGPIOConfigurationHandler = nullptr; + virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) override; + std::function SetGPIOConfigurationHandler = nullptr; + virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) override; + std::function GetGPIOStateHandler = nullptr; + virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) override; + std::function SetGPIOStateHandler = nullptr; + virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) override; + std::function GetI2CConfigurationHandler = nullptr; + virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) override; + std::function SetI2CConfigurationHandler = nullptr; + virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) override; + std::function I2CReadHandler = nullptr; + virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) override; + std::function I2CReadRegisterHandler = nullptr; + virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) override; + std::function I2CWriteHandler = nullptr; + virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) override; + std::function I2CWriteRegisterHandler = nullptr; + virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h new file mode 100644 index 00000000..c98151ff --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h @@ -0,0 +1,58 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VISIONCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_VISIONCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/visionconfig_services_interface.h" + +using namespace std; + +class VisionConfigSimulationServices : public IVisionConfigServices +{ + public: + VisionConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function SetSensorSettingsHandler = nullptr; + virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) override; + std::function GetSensorSettingsHandler = nullptr; + virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) override; + std::function GetOptionValueHandler = nullptr; + virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) override; + std::function SetOptionValueHandler = nullptr; + virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) override; + std::function GetOptionInformationHandler = nullptr; + virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) override; + std::function OnNotificationVisionTopicHandler = nullptr; + virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) override; + virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) override; + std::function DoSensorFocusActionHandler = nullptr; + virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) override; + std::function GetIntrinsicParametersHandler = nullptr; + virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) override; + std::function GetIntrinsicParametersProfileHandler = nullptr; + virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) override; + std::function SetIntrinsicParametersHandler = nullptr; + virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) override; + std::function GetExtrinsicParametersHandler = nullptr; + virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) override; + std::function SetExtrinsicParametersHandler = nullptr; + virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/visionconfig_services.h deleted file mode 100644 index 1b4be317..00000000 --- a/kortex_driver/include/kortex_driver/generated/visionconfig_services.h +++ /dev/null @@ -1,99 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_VISIONCONFIG_SERVICES_H_ -#define _KORTEX_VISIONCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/SetSensorSettings.h" -#include "kortex_driver/GetSensorSettings.h" -#include "kortex_driver/GetOptionValue.h" -#include "kortex_driver/SetOptionValue.h" -#include "kortex_driver/GetOptionInformation.h" -#include "kortex_driver/OnNotificationVisionTopic.h" -#include "kortex_driver/VisionNotification.h" -#include "kortex_driver/DoSensorFocusAction.h" -#include "kortex_driver/GetIntrinsicParameters.h" -#include "kortex_driver/GetIntrinsicParametersProfile.h" -#include "kortex_driver/SetIntrinsicParameters.h" -#include "kortex_driver/GetExtrinsicParameters.h" -#include "kortex_driver/SetExtrinsicParameters.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class VisionConfigServices -{ - public: - VisionConfigServices(ros::NodeHandle& n, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res); - bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res); - bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res); - bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res); - bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res); - bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res); - void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif); - bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res); - bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res); - bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res); - bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res); - bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res); - bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::VisionConfig::VisionConfigClient* m_visionconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_VisionTopic; - bool m_is_activated_VisionTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceSetSensorSettings; - ros::ServiceServer m_serviceGetSensorSettings; - ros::ServiceServer m_serviceGetOptionValue; - ros::ServiceServer m_serviceSetOptionValue; - ros::ServiceServer m_serviceGetOptionInformation; - ros::ServiceServer m_serviceOnNotificationVisionTopic; - ros::ServiceServer m_serviceDoSensorFocusAction; - ros::ServiceServer m_serviceGetIntrinsicParameters; - ros::ServiceServer m_serviceGetIntrinsicParametersProfile; - ros::ServiceServer m_serviceSetIntrinsicParameters; - ros::ServiceServer m_serviceGetExtrinsicParameters; - ros::ServiceServer m_serviceSetExtrinsicParameters; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 09595b86..e15da237 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -35,19 +35,28 @@ #include "kortex_driver/non-generated/kortex_math_util.h" #include "kortex_driver/BaseCyclic_Feedback.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" - -#include "kortex_driver/generated/actuatorconfig_services.h" -#include "kortex_driver/generated/base_services.h" -#include "kortex_driver/generated/deviceconfig_services.h" -#include "kortex_driver/generated/devicemanager_services.h" -#include "kortex_driver/generated/interconnectconfig_services.h" -#include "kortex_driver/generated/visionconfig_services.h" -#include "kortex_driver/generated/controlconfig_services.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" + +#include "kortex_driver/generated/robot/actuatorconfig_services.h" +#include "kortex_driver/generated/robot/base_services.h" +#include "kortex_driver/generated/robot/deviceconfig_services.h" +#include "kortex_driver/generated/robot/devicemanager_services.h" +#include "kortex_driver/generated/robot/interconnectconfig_services.h" +#include "kortex_driver/generated/robot/visionconfig_services.h" +#include "kortex_driver/generated/robot/controlconfig_services.h" + +#include "kortex_driver/generated/simulation/actuatorconfig_services.h" +#include "kortex_driver/generated/simulation/base_services.h" +#include "kortex_driver/generated/simulation/deviceconfig_services.h" +#include "kortex_driver/generated/simulation/devicemanager_services.h" +#include "kortex_driver/generated/simulation/interconnectconfig_services.h" +#include "kortex_driver/generated/simulation/visionconfig_services.h" +#include "kortex_driver/generated/simulation/controlconfig_services.h" #include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" #include "kortex_driver/non-generated/kortex_subscribers.h" +#include "kortex_driver/non-generated/kortex_arm_simulation.h" #define TCP_PORT 10000 #define UDP_PORT 10001 @@ -75,6 +84,10 @@ class KortexArmDriver ros::NodeHandle m_node_handle; + // False if in simulation + bool m_is_real_robot; + std::unique_ptr m_simulator; + // Api options std::string m_ip_address; std::string m_username; @@ -122,13 +135,13 @@ class KortexArmDriver Kinova::Api::SessionManager* m_udp_session_manager; // ROS ServiceProxy's - ActuatorConfigServices* m_actuator_config_ros_services; - BaseServices* m_base_ros_services; - ControlConfigServices* m_control_config_ros_services; - DeviceConfigServices* m_device_config_ros_services; - DeviceManagerServices* m_device_manager_ros_services; - InterconnectConfigServices* m_interconnect_config_ros_services; - VisionConfigServices* m_vision_config_ros_services; + IActuatorConfigServices* m_actuator_config_ros_services; + IBaseServices* m_base_ros_services; + IControlConfigServices* m_control_config_ros_services; + IDeviceConfigServices* m_device_config_ros_services; + IDeviceManagerServices* m_device_manager_ros_services; + IInterconnectConfigServices* m_interconnect_config_ros_services; + IVisionConfigServices* m_vision_config_ros_services; // Action servers PreComputedJointTrajectoryActionServer* m_action_server_follow_joint_trajectory; @@ -149,7 +162,9 @@ class KortexArmDriver // Private methods bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); - void publishFeedback(); + void publishRobotFeedback(); + void publishSimulationFeedback(); + void registerSimulationHandlers(); }; #endif diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h new file mode 100644 index 00000000..2124151d --- /dev/null +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -0,0 +1,210 @@ +#ifndef _KORTEX_ARM_SIMULATION_H_ +#define _KORTEX_ARM_SIMULATION_H_ + +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2020 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +// ROS +#include +#include +#include +#include +#include +#include + +// MoveIt +#include + +// KDL +#include +#include +#include +#include +#include + +// Standard +#include +#include +#include + +// Kortex +#include "kortex_driver/non-generated/kortex_math_util.h" + +#include "kortex_driver/ActionType.h" +#include "kortex_driver/KortexError.h" +#include "kortex_driver/BaseCyclic_Feedback.h" + +#include "kortex_driver/CreateAction.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ReadAllActions.h" +#include "kortex_driver/DeleteAction.h" +#include "kortex_driver/UpdateAction.h" +#include "kortex_driver/ExecuteActionFromReference.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/PauseAction.h" +#include "kortex_driver/StopAction.h" +#include "kortex_driver/ResumeAction.h" + +#include "kortex_driver/PlayCartesianTrajectory.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/GetMeasuredCartesianPose.h" +#include "kortex_driver/SendTwistCommand.h" +#include "kortex_driver/PlayJointTrajectory.h" +#include "kortex_driver/SendJointSpeedsCommand.h" +#include "kortex_driver/SendGripperCommand.h" +#include "kortex_driver/ApplyEmergencyStop.h" + +enum class ControllerType +{ + kTrajectory, // this is for the JointTrajectoryController + kIndividual // this is for the individual JointPositionController's +}; + +class KortexArmSimulation +{ + public: + KortexArmSimulation() = delete; + KortexArmSimulation(ros::NodeHandle& nh); + ~KortexArmSimulation(); + std::unordered_map GetActionsMap() const; + int GetDOF() const {return m_degrees_of_freedom;} + + kortex_driver::BaseCyclic_Feedback GetFeedback(); + + // Handlers for simulated Kortex API functions + // Actions API + kortex_driver::CreateAction::Response CreateAction(const kortex_driver::CreateAction::Request& req); + kortex_driver::ReadAction::Response ReadAction(const kortex_driver::ReadAction::Request& req); + kortex_driver::ReadAllActions::Response ReadAllActions(const kortex_driver::ReadAllActions::Request& req); + kortex_driver::DeleteAction::Response DeleteAction(const kortex_driver::DeleteAction::Request& req); + kortex_driver::UpdateAction::Response UpdateAction(const kortex_driver::UpdateAction::Request& req); + kortex_driver::ExecuteActionFromReference::Response ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req); + kortex_driver::ExecuteAction::Response ExecuteAction(const kortex_driver::ExecuteAction::Request& req); + kortex_driver::StopAction::Response StopAction(const kortex_driver::StopAction::Request& req); + // Other RPCs + kortex_driver::PlayCartesianTrajectory::Response PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req); + kortex_driver::SendTwistCommand::Response SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req); + kortex_driver::PlayJointTrajectory::Response PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req); + kortex_driver::SendJointSpeedsCommand::Response SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req); + kortex_driver::SendGripperCommand::Response SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req); + kortex_driver::Stop::Response Stop(const kortex_driver::Stop::Request& req); + kortex_driver::ApplyEmergencyStop::Response ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req); + + private: + // ROS + ros::NodeHandle m_node_handle; + + // Publishers + ros::Publisher m_pub_action_topic; + std::vector m_pub_position_controllers; + + // Subscribers + ros::Subscriber m_sub_joint_trajectory_controller_state; + ros::Subscriber m_sub_joint_state; + ros::Subscriber m_sub_joint_speeds; + ros::Subscriber m_sub_twist; + ros::Subscriber m_sub_clear_faults; + ros::Subscriber m_sub_stop; + ros::Subscriber m_sub_emergency_stop; + + // Service clients + ros::ServiceClient m_client_switch_controllers; + ControllerType m_active_controller_type; + std::vector m_trajectory_controllers_list; + std::vector m_position_controllers_list; + std::vector m_velocity_commands; + kortex_driver::Twist m_twist_command; + + // Action clients + std::unique_ptr> m_follow_joint_trajectory_action_client; + std::unique_ptr> m_gripper_action_client; + + // Namespacing and prefixing information + std::string m_prefix; + std::string m_robot_name; + + // Arm information + std::string m_arm_name; + std::vector m_arm_joint_names; + std::vector m_arm_joint_limits_min; + std::vector m_arm_joint_limits_max; + std::vector m_arm_velocity_max_limits; + std::vector m_arm_acceleration_max_limits; + float m_max_cartesian_twist_linear; + float m_max_cartesian_twist_angular; + float m_max_cartesian_acceleration_linear; + float m_max_cartesian_acceleration_angular; + + // Gripper information + std::string m_gripper_name; + std::vector m_gripper_joint_names; + std::vector m_gripper_joint_limits_max; + std::vector m_gripper_joint_limits_min; + int m_degrees_of_freedom; + + // The indexes of the first arm joint and of the gripper joint in the "joint_states" feedback + int m_first_arm_joint_index; + int m_gripper_joint_index; + + // Action-related + std::unordered_map m_map_actions; + + // Math utility + KortexMathUtil m_math_util; + + // KDL chain, solvers and motions + KDL::Chain m_chain; + std::unique_ptr m_fk_solver; + std::unique_ptr m_ik_pos_solver; + std::unique_ptr m_ik_vel_solver; + std::vector m_velocity_trap_profiles; + + // Threading + std::atomic m_is_action_being_executed; + std::atomic m_action_preempted; + std::atomic m_current_action_type; + std::thread m_action_executor_thread; + + // MoveIt-related + std::unique_ptr m_moveit_arm_interface; + std::unique_ptr m_moveit_gripper_interface; + + // Subscription callbacks and data structures with their mutexes + void cb_joint_states(const sensor_msgs::JointState& state); + sensor_msgs::JointState m_current_state; + bool m_first_state_received; + kortex_driver::BaseCyclic_Feedback m_feedback; + std::mutex m_state_mutex; + + // Helper functions + bool IsGripperPresent() const {return !m_gripper_name.empty();} + void CreateDefaultActions(); + bool SwitchControllerType(ControllerType new_type); + void PlayAction(const kortex_driver::Action& action); + void JoinThreadAndCancelAction(); + kortex_driver::KortexError FillKortexError(uint32_t code, uint32_t subCode, const std::string& description = std::string()) const; + kortex_driver::KortexError ExecuteReachJointAngles(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteReachPose(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendJointSpeeds(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendTwist(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendGripperCommand(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteTimeDelay(const kortex_driver::Action& action); + + // Callbacks + void new_joint_speeds_cb(const kortex_driver::Base_JointSpeeds& joint_speeds); + void new_twist_cb(const kortex_driver::TwistCommand& twist); + void clear_faults_cb(const std_msgs::Empty& empty); + void stop_cb(const std_msgs::Empty& empty); + void emergency_stop_cb(const std_msgs::Empty& empty); +}; + +#endif //_KORTEX_ARM_SIMULATION_H_ diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h index 9c94d61e..59b4c85d 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h @@ -12,6 +12,7 @@ */ #include +#include "kortex_driver/Twist.h" class KortexMathUtil { @@ -21,10 +22,16 @@ class KortexMathUtil static double toRad(double degree); static double toDeg(double rad); + static int getNumberOfTurns(double rad_not_wrapped); static double wrapRadiansFromMinusPiToPi(double rad_not_wrapped); + static double wrapRadiansFromMinusPiToPi(double rad_not_wrapped, int& number_of_turns); static double wrapDegreesFromZeroTo360(double deg_not_wrapped); + static double wrapDegreesFromZeroTo360(double deg_not_wrapped, int& number_of_turns); static double relative_position_from_absolute(double absolute_position, double min_value, double max_value); static double absolute_position_from_relative(double relative_position, double min_value, double max_value); + + // kortex_driver::Twist helper functions + static kortex_driver::Twist substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b); }; #endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h b/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h index 26e14216..45ed76bd 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h @@ -21,7 +21,7 @@ #include "kortex_driver/TwistCommand.h" #include "kortex_driver/Base_JointSpeeds.h" -#include "kortex_driver/generated/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" #include "kortex_driver/non-generated/kortex_math_util.h" class KortexSubscribers diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 3d6fe61d..ce95d368 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -9,7 +9,9 @@ - + + + @@ -36,19 +38,19 @@ - - - - @@ -56,6 +58,7 @@ + @@ -69,10 +72,11 @@ + - + - + @@ -83,11 +87,13 @@ + + @@ -98,7 +104,7 @@ - + @@ -116,9 +122,9 @@ - + - + diff --git a/kortex_driver/launch/kortex_dual_driver.launch b/kortex_driver/launch/kortex_dual_driver.launch index 0939e816..57b7260c 100644 --- a/kortex_driver/launch/kortex_dual_driver.launch +++ b/kortex_driver/launch/kortex_dual_driver.launch @@ -60,7 +60,7 @@ - + + - + launch-prefix="gdb -ex run args" @@ -109,9 +110,10 @@ - + + - + diff --git a/kortex_driver/launch/test_simulator.launch b/kortex_driver/launch/test_simulator.launch new file mode 100644 index 00000000..b425760b --- /dev/null +++ b/kortex_driver/launch/test_simulator.launch @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [base_feedback/joint_state] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_driver/package.xml b/kortex_driver/package.xml index 3654d29d..757f4407 100644 --- a/kortex_driver/package.xml +++ b/kortex_driver/package.xml @@ -1,10 +1,12 @@ kortex_driver - 2.2.1 - THe kortex package that act as a robot's driver. + 2.2.2 + The package that acts as a robot's driver. Supports simulated and real Kortex robots. - + Hugo Lamontagne + Alexandre Vannobel + BSD @@ -13,7 +15,10 @@ rospy std_msgs control_msgs + controller_manager_msgs actionlib + moveit_ros_planning_interface + kdl_parser roscpp rospy std_msgs diff --git a/kortex_driver/readme.md b/kortex_driver/readme.md index 6e913b2b..47351ffd 100644 --- a/kortex_driver/readme.md +++ b/kortex_driver/readme.md @@ -51,7 +51,7 @@ The `kortex_driver` node is the node responsible for the communication between t - **arm** : Name of your robot arm model. See the `kortex_description/arms` folder to see the available robot models. The default value is **gen3**. - **dof** : Number of DOFs of your robot. The default value is for Gen3 is **7** and the default value for Gen3 lite is **6**. You will have to specify this value only if you have a Gen3 6DOF. - **vision** : Boolean value to indicate if your arm has a Vision Module. The default value is for Gen3 is **true** and the default value for Gen3 lite is **false**. You will have to specify this value only if you have a Gen3 6DOF without a Vision Module. This argument only affects the visual representation of the arm in RViz. -- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85**. For Gen3 lite, you need to put **gen3_lite_2f**. +- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85** or **robotiq_2f_140**. For Gen3 lite, the default (and only) gripper is **gen3_lite_2f**. - **robot_name** : This is the namespace in which the driver will run. It defaults to **my_$(arg arm)** (so "my_gen3" for arm="gen3"). - **prefix** : This is an optional prefix for all joint and link names in the kortex_description. It is used to allow differentiating between different arms in the same URDF. It defaults to **an empty string**. **Note** : Changing the prefix invalidates the MoveIt! configuration, and requires modifying said configuration, plus the .yaml files with harcoded joint names. - **ip_address** : The IP address of the robot you're connecting to. The default value is **192.168.1.10**. @@ -80,6 +80,8 @@ You can also have a look at the [roslaunch documentation](http://wiki.ros.org/ro If everything goes well, you will see a "**The Kortex driver has been initialized correctly!**" message. If you also start MoveIt!, the `kortex_driver` output may be flooded in the `move_group` output, so pay attention to the warning and error messages! If the node fails to start for any reason, you will get an error message followed by a "**process has died**" message. +You will read below about the topics and services the driver offers. To read more about how to use those tools, [go to the kortex_examples documentation](../kortex_examples/readme.md). + ## Topics diff --git a/kortex_driver/scripts/ros_kortex_generator.py b/kortex_driver/scripts/ros_kortex_generator.py index 7bc002a7..8d46ad33 100755 --- a/kortex_driver/scripts/ros_kortex_generator.py +++ b/kortex_driver/scripts/ros_kortex_generator.py @@ -377,8 +377,11 @@ def generate_code(request, response): os.makedirs("../{}/generated/{}".format(s, package.short_name_lowercase_with_underscores)) shutil.rmtree("../src/generated", ignore_errors=True) shutil.rmtree("../include/kortex_driver/generated", ignore_errors=True) - os.makedirs("../src/generated") - os.makedirs("../include/kortex_driver/generated") + os.makedirs("../src/generated/robot") + os.makedirs("../src/generated/simulation") + os.makedirs("../include/kortex_driver/generated/interfaces") + os.makedirs("../include/kortex_driver/generated/robot") + os.makedirs("../include/kortex_driver/generated/simulation") ########################################### # Parse the proto files to add the messages and RPC's to the DetailedPackage's @@ -453,7 +456,7 @@ def generate_code(request, response): include_file_names = [] for p in packages_with_messages: for s in ["proto", "ros"]: - include_file_names.append("kortex_driver/generated/{}_{}_converter.h".format(p.short_name.lower(), s)) + include_file_names.append("kortex_driver/generated/robot/{}_{}_converter.h".format(p.short_name.lower(), s)) # Generate the ROS files for each package for package in packages_dict.values(): @@ -498,33 +501,50 @@ def generate_code(request, response): if package.messages: # package contains at least one message # Proto converter header file - current_header_filename = "kortex_driver/generated/{}_proto_converter.h".format(package.short_name.lower()) + current_header_filename = "kortex_driver/generated/robot/{}_proto_converter.h".format(package.short_name.lower()) this_package_context.include_file_names = filter(lambda x : "proto_converter" in x and x != current_header_filename, include_file_names) with open(os.path.join("..", "include/" + current_header_filename), 'wt') as converterFile: converterFile.write(render("../templates/proto_converter.h.jinja2", this_package_context.__dict__)) # Proto converter cpp file this_package_context.current_header_filename = current_header_filename - with open(os.path.join("..", "src/generated/{}_proto_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: + with open(os.path.join("..", "src/generated/robot/{}_proto_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: converterFile.write(render("../templates/proto_converter.cpp.jinja2", this_package_context.__dict__)) # ROS converter header file - current_header_filename = "kortex_driver/generated/{}_ros_converter.h".format(package.short_name.lower()) + current_header_filename = "kortex_driver/generated/robot/{}_ros_converter.h".format(package.short_name.lower()) this_package_context.include_file_names = filter(lambda x : "ros_converter" in x and x != current_header_filename, include_file_names) with open(os.path.join("..", "include/" + current_header_filename), 'wt') as converterFile: converterFile.write(render("../templates/ros_converter.h.jinja2", this_package_context.__dict__)) # ROS converter cpp file this_package_context.current_header_filename = current_header_filename - with open(os.path.join("..", "src/generated/{}_ros_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: + with open(os.path.join("..", "src/generated/robot/{}_ros_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: converterFile.write(render("../templates/ros_converter.cpp.jinja2", this_package_context.__dict__)) # Generate the ServiceProxy's for every Kortex API method if package.methods: # package contains at least one RPC - current_header_filename = "kortex_driver/generated/{}_services.h".format(package.short_name.lower()) + # Generate interface files + current_header_filename = "kortex_driver/generated/interfaces/{}_services_interface.h".format(package.short_name.lower()) + current_interface_header_filename = current_header_filename + this_package_context.current_interface_header_filename = current_interface_header_filename + with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: + services_file.write(render("../templates/services_interface.h.jinja2", this_package_context.__dict__)) + + # Generate robot files + current_header_filename = "kortex_driver/generated/robot/{}_services.h".format(package.short_name.lower()) + this_package_context.current_header_filename = current_header_filename + this_package_context.include_file_names = include_file_names + with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: + services_file.write(render("../templates/services_robot.h.jinja2", this_package_context.__dict__)) + with open(os.path.join("..", "src/generated/robot/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: + services_file.write(render("../templates/services_robot.cpp.jinja2", this_package_context.__dict__)) + + # Generate simulation files + current_header_filename = "kortex_driver/generated/simulation/{}_services.h".format(package.short_name.lower()) this_package_context.current_header_filename = current_header_filename this_package_context.include_file_names = include_file_names with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: - services_file.write(render("../templates/services.h.jinja2", this_package_context.__dict__)) - with open(os.path.join("..", "src/generated/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: - services_file.write(render("../templates/services.cpp.jinja2", this_package_context.__dict__)) + services_file.write(render("../templates/services_simulation.h.jinja2", this_package_context.__dict__)) + with open(os.path.join("..", "src/generated/simulation/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: + services_file.write(render("../templates/services_simulation.cpp.jinja2", this_package_context.__dict__)) # Delete unused folders we created for None for package in packages_dict.values(): diff --git a/kortex_driver/src/generated/devicemanager_services.cpp b/kortex_driver/src/generated/devicemanager_services.cpp deleted file mode 100644 index c544c00e..00000000 --- a/kortex_driver/src/generated/devicemanager_services.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_services.h" - -DeviceManagerServices::DeviceManagerServices(ros::NodeHandle& n, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms): - m_n(n), - m_devicemanager(devicemanager), - m_current_device_id(device_id) -{ - m_api_options.timeout_ms = timeout_ms; - - m_pub_Error = m_n.advertise("kortex_error", 1000); - - m_serviceSetDeviceID = n.advertiseService("device_manager/set_device_id", &DeviceManagerServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("device_manager/set_api_options", &DeviceManagerServices::SetApiOptions, this); - - m_serviceReadAllDevices = m_n.advertiseService("device_manager/read_all_devices", &DeviceManagerServices::ReadAllDevices, this); -} - -bool DeviceManagerServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) -{ - m_current_device_id = req.device_id; - - return true; -} - -bool DeviceManagerServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) -{ - m_api_options.timeout_ms = req.input.timeout_ms; - - return true; -} - - -bool DeviceManagerServices::ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) -{ - - Kinova::Api::DeviceManager::DeviceHandles output; - - kortex_driver::KortexError result_error; - - try - { - output = m_devicemanager->ReadAllDevices(m_current_device_id, m_api_options); - } - - catch (Kinova::Api::KDetailedException& ex) - { - result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); - result_error.code = ex.getErrorInfo().getError().error_code(); - result_error.description = ex.toString(); - m_pub_Error.publish(result_error); - ROS_INFO("Kortex exception"); - ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); - ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); - ROS_INFO("KINOVA exception description: %s\n", ex.what()); - return false; - } - catch (std::runtime_error& ex2) - { - ROS_INFO("%s", ex2.what()); - return false; - } - ToRosData(output, res.output); - return true; -} diff --git a/kortex_driver/src/generated/actuatorconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/actuatorconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/actuatorconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/actuatorconfig_proto_converter.cpp index 4d52e169..454a5faa 100644 --- a/kortex_driver/src/generated/actuatorconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" int ToProtoData(kortex_driver::AxisPosition input, Kinova::Api::ActuatorConfig::AxisPosition *output) { diff --git a/kortex_driver/src/generated/actuatorconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/actuatorconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/actuatorconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/actuatorconfig_ros_converter.cpp index 054b2ea4..d6fbfeab 100644 --- a/kortex_driver/src/generated/actuatorconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" int ToRosData(Kinova::Api::ActuatorConfig::AxisPosition input, kortex_driver::AxisPosition &output) { diff --git a/kortex_driver/src/generated/actuatorconfig_services.cpp b/kortex_driver/src/generated/robot/actuatorconfig_services.cpp similarity index 67% rename from kortex_driver/src/generated/actuatorconfig_services.cpp rename to kortex_driver/src/generated/robot/actuatorconfig_services.cpp index 39aca451..e9a39715 100644 --- a/kortex_driver/src/generated/actuatorconfig_services.cpp +++ b/kortex_driver/src/generated/robot/actuatorconfig_services.cpp @@ -14,76 +14,76 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_services.h" - -ActuatorConfigServices::ActuatorConfigServices(ros::NodeHandle& n, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_services.h" + +ActuatorConfigRobotServices::ActuatorConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms): + IActuatorConfigServices(node_handle), m_actuatorconfig(actuatorconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - - m_serviceSetDeviceID = n.advertiseService("actuator_config/set_device_id", &ActuatorConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("actuator_config/set_api_options", &ActuatorConfigServices::SetApiOptions, this); - - m_serviceGetAxisOffsets = m_n.advertiseService("actuator_config/get_axis_offsets", &ActuatorConfigServices::GetAxisOffsets, this); - m_serviceSetAxisOffsets = m_n.advertiseService("actuator_config/set_axis_offsets", &ActuatorConfigServices::SetAxisOffsets, this); - m_serviceSetTorqueOffset = m_n.advertiseService("actuator_config/set_torque_offset", &ActuatorConfigServices::SetTorqueOffset, this); - m_serviceActuatorConfig_GetControlMode = m_n.advertiseService("actuator_config/get_control_mode", &ActuatorConfigServices::ActuatorConfig_GetControlMode, this); - m_serviceSetControlMode = m_n.advertiseService("actuator_config/set_control_mode", &ActuatorConfigServices::SetControlMode, this); - m_serviceGetActivatedControlLoop = m_n.advertiseService("actuator_config/get_activated_control_loop", &ActuatorConfigServices::GetActivatedControlLoop, this); - m_serviceSetActivatedControlLoop = m_n.advertiseService("actuator_config/set_activated_control_loop", &ActuatorConfigServices::SetActivatedControlLoop, this); - m_serviceGetControlLoopParameters = m_n.advertiseService("actuator_config/get_control_loop_parameters", &ActuatorConfigServices::GetControlLoopParameters, this); - m_serviceSetControlLoopParameters = m_n.advertiseService("actuator_config/set_control_loop_parameters", &ActuatorConfigServices::SetControlLoopParameters, this); - m_serviceSelectCustomData = m_n.advertiseService("actuator_config/select_custom_data", &ActuatorConfigServices::SelectCustomData, this); - m_serviceGetSelectedCustomData = m_n.advertiseService("actuator_config/get_selected_custom_data", &ActuatorConfigServices::GetSelectedCustomData, this); - m_serviceSetCommandMode = m_n.advertiseService("actuator_config/set_command_mode", &ActuatorConfigServices::SetCommandMode, this); - m_serviceActuatorConfig_ClearFaults = m_n.advertiseService("actuator_config/clear_faults", &ActuatorConfigServices::ActuatorConfig_ClearFaults, this); - m_serviceSetServoing = m_n.advertiseService("actuator_config/set_servoing", &ActuatorConfigServices::SetServoing, this); - m_serviceMoveToPosition = m_n.advertiseService("actuator_config/move_to_position", &ActuatorConfigServices::MoveToPosition, this); - m_serviceGetCommandMode = m_n.advertiseService("actuator_config/get_command_mode", &ActuatorConfigServices::GetCommandMode, this); - m_serviceGetServoing = m_n.advertiseService("actuator_config/get_servoing", &ActuatorConfigServices::GetServoing, this); - m_serviceGetTorqueOffset = m_n.advertiseService("actuator_config/get_torque_offset", &ActuatorConfigServices::GetTorqueOffset, this); - m_serviceSetCoggingFeedforwardMode = m_n.advertiseService("actuator_config/set_cogging_feedforward_mode", &ActuatorConfigServices::SetCoggingFeedforwardMode, this); - m_serviceGetCoggingFeedforwardMode = m_n.advertiseService("actuator_config/get_cogging_feedforward_mode", &ActuatorConfigServices::GetCoggingFeedforwardMode, this); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("actuator_config/set_device_id", &ActuatorConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("actuator_config/set_api_options", &ActuatorConfigRobotServices::SetApiOptions, this); + + m_serviceGetAxisOffsets = m_node_handle.advertiseService("actuator_config/get_axis_offsets", &ActuatorConfigRobotServices::GetAxisOffsets, this); + m_serviceSetAxisOffsets = m_node_handle.advertiseService("actuator_config/set_axis_offsets", &ActuatorConfigRobotServices::SetAxisOffsets, this); + m_serviceSetTorqueOffset = m_node_handle.advertiseService("actuator_config/set_torque_offset", &ActuatorConfigRobotServices::SetTorqueOffset, this); + m_serviceActuatorConfig_GetControlMode = m_node_handle.advertiseService("actuator_config/get_control_mode", &ActuatorConfigRobotServices::ActuatorConfig_GetControlMode, this); + m_serviceSetControlMode = m_node_handle.advertiseService("actuator_config/set_control_mode", &ActuatorConfigRobotServices::SetControlMode, this); + m_serviceGetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/get_activated_control_loop", &ActuatorConfigRobotServices::GetActivatedControlLoop, this); + m_serviceSetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/set_activated_control_loop", &ActuatorConfigRobotServices::SetActivatedControlLoop, this); + m_serviceGetControlLoopParameters = m_node_handle.advertiseService("actuator_config/get_control_loop_parameters", &ActuatorConfigRobotServices::GetControlLoopParameters, this); + m_serviceSetControlLoopParameters = m_node_handle.advertiseService("actuator_config/set_control_loop_parameters", &ActuatorConfigRobotServices::SetControlLoopParameters, this); + m_serviceSelectCustomData = m_node_handle.advertiseService("actuator_config/select_custom_data", &ActuatorConfigRobotServices::SelectCustomData, this); + m_serviceGetSelectedCustomData = m_node_handle.advertiseService("actuator_config/get_selected_custom_data", &ActuatorConfigRobotServices::GetSelectedCustomData, this); + m_serviceSetCommandMode = m_node_handle.advertiseService("actuator_config/set_command_mode", &ActuatorConfigRobotServices::SetCommandMode, this); + m_serviceActuatorConfig_ClearFaults = m_node_handle.advertiseService("actuator_config/clear_faults", &ActuatorConfigRobotServices::ActuatorConfig_ClearFaults, this); + m_serviceSetServoing = m_node_handle.advertiseService("actuator_config/set_servoing", &ActuatorConfigRobotServices::SetServoing, this); + m_serviceMoveToPosition = m_node_handle.advertiseService("actuator_config/move_to_position", &ActuatorConfigRobotServices::MoveToPosition, this); + m_serviceGetCommandMode = m_node_handle.advertiseService("actuator_config/get_command_mode", &ActuatorConfigRobotServices::GetCommandMode, this); + m_serviceGetServoing = m_node_handle.advertiseService("actuator_config/get_servoing", &ActuatorConfigRobotServices::GetServoing, this); + m_serviceGetTorqueOffset = m_node_handle.advertiseService("actuator_config/get_torque_offset", &ActuatorConfigRobotServices::GetTorqueOffset, this); + m_serviceSetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/set_cogging_feedforward_mode", &ActuatorConfigRobotServices::SetCoggingFeedforwardMode, this); + m_serviceGetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/get_cogging_feedforward_mode", &ActuatorConfigRobotServices::GetCoggingFeedforwardMode, this); } -bool ActuatorConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool ActuatorConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool ActuatorConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool ActuatorConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -91,7 +91,7 @@ bool ActuatorConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool ActuatorConfigServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) +bool ActuatorConfigRobotServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) { Kinova::Api::ActuatorConfig::AxisOffsets output; @@ -124,7 +124,7 @@ bool ActuatorConfigServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Reque return true; } -bool ActuatorConfigServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) +bool ActuatorConfigRobotServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) { Kinova::Api::ActuatorConfig::AxisPosition input; @@ -156,7 +156,7 @@ bool ActuatorConfigServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Reque return true; } -bool ActuatorConfigServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) +bool ActuatorConfigRobotServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) { Kinova::Api::ActuatorConfig::TorqueOffset input; @@ -188,7 +188,7 @@ bool ActuatorConfigServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Req return true; } -bool ActuatorConfigServices::ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) +bool ActuatorConfigRobotServices::ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) { Kinova::Api::ActuatorConfig::ControlModeInformation output; @@ -221,7 +221,7 @@ bool ActuatorConfigServices::ActuatorConfig_GetControlMode(kortex_driver::Actuat return true; } -bool ActuatorConfigServices::SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) +bool ActuatorConfigRobotServices::SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) { Kinova::Api::ActuatorConfig::ControlModeInformation input; @@ -253,7 +253,7 @@ bool ActuatorConfigServices::SetControlMode(kortex_driver::SetControlMode::Reque return true; } -bool ActuatorConfigServices::GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) +bool ActuatorConfigRobotServices::GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) { Kinova::Api::ActuatorConfig::ControlLoop output; @@ -286,7 +286,7 @@ bool ActuatorConfigServices::GetActivatedControlLoop(kortex_driver::GetActivated return true; } -bool ActuatorConfigServices::SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) +bool ActuatorConfigRobotServices::SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) { Kinova::Api::ActuatorConfig::ControlLoop input; @@ -318,7 +318,7 @@ bool ActuatorConfigServices::SetActivatedControlLoop(kortex_driver::SetActivated return true; } -bool ActuatorConfigServices::GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) +bool ActuatorConfigRobotServices::GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) { Kinova::Api::ActuatorConfig::LoopSelection input; @@ -353,7 +353,7 @@ bool ActuatorConfigServices::GetControlLoopParameters(kortex_driver::GetControlL return true; } -bool ActuatorConfigServices::SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) +bool ActuatorConfigRobotServices::SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) { Kinova::Api::ActuatorConfig::ControlLoopParameters input; @@ -385,7 +385,7 @@ bool ActuatorConfigServices::SetControlLoopParameters(kortex_driver::SetControlL return true; } -bool ActuatorConfigServices::SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) +bool ActuatorConfigRobotServices::SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) { Kinova::Api::ActuatorConfig::CustomDataSelection input; @@ -417,7 +417,7 @@ bool ActuatorConfigServices::SelectCustomData(kortex_driver::SelectCustomData::R return true; } -bool ActuatorConfigServices::GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) +bool ActuatorConfigRobotServices::GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) { Kinova::Api::ActuatorConfig::CustomDataSelection output; @@ -450,7 +450,7 @@ bool ActuatorConfigServices::GetSelectedCustomData(kortex_driver::GetSelectedCus return true; } -bool ActuatorConfigServices::SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) +bool ActuatorConfigRobotServices::SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) { Kinova::Api::ActuatorConfig::CommandModeInformation input; @@ -482,7 +482,7 @@ bool ActuatorConfigServices::SetCommandMode(kortex_driver::SetCommandMode::Reque return true; } -bool ActuatorConfigServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) +bool ActuatorConfigRobotServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) { kortex_driver::KortexError result_error; @@ -512,7 +512,7 @@ bool ActuatorConfigServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorC return true; } -bool ActuatorConfigServices::SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) +bool ActuatorConfigRobotServices::SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) { Kinova::Api::ActuatorConfig::Servoing input; @@ -544,7 +544,7 @@ bool ActuatorConfigServices::SetServoing(kortex_driver::SetServoing::Request &r return true; } -bool ActuatorConfigServices::MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) +bool ActuatorConfigRobotServices::MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) { Kinova::Api::ActuatorConfig::PositionCommand input; @@ -576,7 +576,7 @@ bool ActuatorConfigServices::MoveToPosition(kortex_driver::MoveToPosition::Reque return true; } -bool ActuatorConfigServices::GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) +bool ActuatorConfigRobotServices::GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) { Kinova::Api::ActuatorConfig::CommandModeInformation output; @@ -609,7 +609,7 @@ bool ActuatorConfigServices::GetCommandMode(kortex_driver::GetCommandMode::Reque return true; } -bool ActuatorConfigServices::GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) +bool ActuatorConfigRobotServices::GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) { Kinova::Api::ActuatorConfig::Servoing output; @@ -642,7 +642,7 @@ bool ActuatorConfigServices::GetServoing(kortex_driver::GetServoing::Request &r return true; } -bool ActuatorConfigServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) +bool ActuatorConfigRobotServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) { Kinova::Api::ActuatorConfig::TorqueOffset output; @@ -675,7 +675,7 @@ bool ActuatorConfigServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Req return true; } -bool ActuatorConfigServices::SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) +bool ActuatorConfigRobotServices::SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) { Kinova::Api::ActuatorConfig::CoggingFeedforwardModeInformation input; @@ -707,7 +707,7 @@ bool ActuatorConfigServices::SetCoggingFeedforwardMode(kortex_driver::SetCogging return true; } -bool ActuatorConfigServices::GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) +bool ActuatorConfigRobotServices::GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) { Kinova::Api::ActuatorConfig::CoggingFeedforwardModeInformation output; diff --git a/kortex_driver/src/generated/actuatorcyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/actuatorcyclic_proto_converter.cpp similarity index 97% rename from kortex_driver/src/generated/actuatorcyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/actuatorcyclic_proto_converter.cpp index 7a1ce565..6d7744fc 100644 --- a/kortex_driver/src/generated/actuatorcyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorcyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" int ToProtoData(kortex_driver::ActuatorCyclic_MessageId input, Kinova::Api::ActuatorCyclic::MessageId *output) { diff --git a/kortex_driver/src/generated/actuatorcyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/actuatorcyclic_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/actuatorcyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/actuatorcyclic_ros_converter.cpp index d54c6f17..4e2e5ef3 100644 --- a/kortex_driver/src/generated/actuatorcyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorcyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" int ToRosData(Kinova::Api::ActuatorCyclic::MessageId input, kortex_driver::ActuatorCyclic_MessageId &output) { diff --git a/kortex_driver/src/generated/base_proto_converter.cpp b/kortex_driver/src/generated/robot/base_proto_converter.cpp similarity index 99% rename from kortex_driver/src/generated/base_proto_converter.cpp rename to kortex_driver/src/generated/robot/base_proto_converter.cpp index 9e1d250f..1cefcb7b 100644 --- a/kortex_driver/src/generated/base_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/base_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" int ToProtoData(kortex_driver::FullUserProfile input, Kinova::Api::Base::FullUserProfile *output) { diff --git a/kortex_driver/src/generated/base_ros_converter.cpp b/kortex_driver/src/generated/robot/base_ros_converter.cpp similarity index 99% rename from kortex_driver/src/generated/base_ros_converter.cpp rename to kortex_driver/src/generated/robot/base_ros_converter.cpp index e8c1f2d4..6ac426c1 100644 --- a/kortex_driver/src/generated/base_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/base_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/base_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" int ToRosData(Kinova::Api::Base::FullUserProfile input, kortex_driver::FullUserProfile &output) { diff --git a/kortex_driver/src/generated/base_services.cpp b/kortex_driver/src/generated/robot/base_services.cpp similarity index 73% rename from kortex_driver/src/generated/base_services.cpp rename to kortex_driver/src/generated/robot/base_services.cpp index b40f51d2..1a8a6dd9 100644 --- a/kortex_driver/src/generated/base_services.cpp +++ b/kortex_driver/src/generated/robot/base_services.cpp @@ -14,228 +14,228 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/base_services.h" - -BaseServices::BaseServices(ros::NodeHandle& n, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/base_services.h" + +BaseRobotServices::BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms): + IBaseServices(node_handle), m_base(base), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_ConfigurationChangeTopic = m_n.advertise("configuration_change_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ConfigurationChangeTopic = m_node_handle.advertise("configuration_change_topic", 1000); m_is_activated_ConfigurationChangeTopic = false; - m_pub_MappingInfoTopic = m_n.advertise("mapping_info_topic", 1000); + m_pub_MappingInfoTopic = m_node_handle.advertise("mapping_info_topic", 1000); m_is_activated_MappingInfoTopic = false; - m_pub_ControlModeTopic = m_n.advertise("control_mode_topic", 1000); + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); m_is_activated_ControlModeTopic = false; - m_pub_OperatingModeTopic = m_n.advertise("operating_mode_topic", 1000); + m_pub_OperatingModeTopic = m_node_handle.advertise("operating_mode_topic", 1000); m_is_activated_OperatingModeTopic = false; - m_pub_SequenceInfoTopic = m_n.advertise("sequence_info_topic", 1000); + m_pub_SequenceInfoTopic = m_node_handle.advertise("sequence_info_topic", 1000); m_is_activated_SequenceInfoTopic = false; - m_pub_ProtectionZoneTopic = m_n.advertise("protection_zone_topic", 1000); + m_pub_ProtectionZoneTopic = m_node_handle.advertise("protection_zone_topic", 1000); m_is_activated_ProtectionZoneTopic = false; - m_pub_UserTopic = m_n.advertise("user_topic", 1000); + m_pub_UserTopic = m_node_handle.advertise("user_topic", 1000); m_is_activated_UserTopic = false; - m_pub_ControllerTopic = m_n.advertise("controller_topic", 1000); + m_pub_ControllerTopic = m_node_handle.advertise("controller_topic", 1000); m_is_activated_ControllerTopic = false; - m_pub_ActionTopic = m_n.advertise("action_topic", 1000); + m_pub_ActionTopic = m_node_handle.advertise("action_topic", 1000); m_is_activated_ActionTopic = false; - m_pub_RobotEventTopic = m_n.advertise("robot_event_topic", 1000); + m_pub_RobotEventTopic = m_node_handle.advertise("robot_event_topic", 1000); m_is_activated_RobotEventTopic = false; - m_pub_ServoingModeTopic = m_n.advertise("servoing_mode_topic", 1000); + m_pub_ServoingModeTopic = m_node_handle.advertise("servoing_mode_topic", 1000); m_is_activated_ServoingModeTopic = false; - m_pub_FactoryTopic = m_n.advertise("factory_topic", 1000); + m_pub_FactoryTopic = m_node_handle.advertise("factory_topic", 1000); m_is_activated_FactoryTopic = false; - m_pub_NetworkTopic = m_n.advertise("network_topic", 1000); + m_pub_NetworkTopic = m_node_handle.advertise("network_topic", 1000); m_is_activated_NetworkTopic = false; - m_pub_ArmStateTopic = m_n.advertise("arm_state_topic", 1000); + m_pub_ArmStateTopic = m_node_handle.advertise("arm_state_topic", 1000); m_is_activated_ArmStateTopic = false; - m_serviceSetDeviceID = n.advertiseService("base/set_device_id", &BaseServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("base/set_api_options", &BaseServices::SetApiOptions, this); - - m_serviceCreateUserProfile = m_n.advertiseService("base/create_user_profile", &BaseServices::CreateUserProfile, this); - m_serviceUpdateUserProfile = m_n.advertiseService("base/update_user_profile", &BaseServices::UpdateUserProfile, this); - m_serviceReadUserProfile = m_n.advertiseService("base/read_user_profile", &BaseServices::ReadUserProfile, this); - m_serviceDeleteUserProfile = m_n.advertiseService("base/delete_user_profile", &BaseServices::DeleteUserProfile, this); - m_serviceReadAllUserProfiles = m_n.advertiseService("base/read_all_user_profiles", &BaseServices::ReadAllUserProfiles, this); - m_serviceReadAllUsers = m_n.advertiseService("base/read_all_users", &BaseServices::ReadAllUsers, this); - m_serviceChangePassword = m_n.advertiseService("base/change_password", &BaseServices::ChangePassword, this); - m_serviceCreateSequence = m_n.advertiseService("base/create_sequence", &BaseServices::CreateSequence, this); - m_serviceUpdateSequence = m_n.advertiseService("base/update_sequence", &BaseServices::UpdateSequence, this); - m_serviceReadSequence = m_n.advertiseService("base/read_sequence", &BaseServices::ReadSequence, this); - m_serviceDeleteSequence = m_n.advertiseService("base/delete_sequence", &BaseServices::DeleteSequence, this); - m_serviceReadAllSequences = m_n.advertiseService("base/read_all_sequences", &BaseServices::ReadAllSequences, this); - m_servicePlaySequence = m_n.advertiseService("base/play_sequence", &BaseServices::PlaySequence, this); - m_servicePlayAdvancedSequence = m_n.advertiseService("base/play_advanced_sequence", &BaseServices::PlayAdvancedSequence, this); - m_serviceStopSequence = m_n.advertiseService("base/stop_sequence", &BaseServices::StopSequence, this); - m_servicePauseSequence = m_n.advertiseService("base/pause_sequence", &BaseServices::PauseSequence, this); - m_serviceResumeSequence = m_n.advertiseService("base/resume_sequence", &BaseServices::ResumeSequence, this); - m_serviceCreateProtectionZone = m_n.advertiseService("base/create_protection_zone", &BaseServices::CreateProtectionZone, this); - m_serviceUpdateProtectionZone = m_n.advertiseService("base/update_protection_zone", &BaseServices::UpdateProtectionZone, this); - m_serviceReadProtectionZone = m_n.advertiseService("base/read_protection_zone", &BaseServices::ReadProtectionZone, this); - m_serviceDeleteProtectionZone = m_n.advertiseService("base/delete_protection_zone", &BaseServices::DeleteProtectionZone, this); - m_serviceReadAllProtectionZones = m_n.advertiseService("base/read_all_protection_zones", &BaseServices::ReadAllProtectionZones, this); - m_serviceCreateMapping = m_n.advertiseService("base/create_mapping", &BaseServices::CreateMapping, this); - m_serviceReadMapping = m_n.advertiseService("base/read_mapping", &BaseServices::ReadMapping, this); - m_serviceUpdateMapping = m_n.advertiseService("base/update_mapping", &BaseServices::UpdateMapping, this); - m_serviceDeleteMapping = m_n.advertiseService("base/delete_mapping", &BaseServices::DeleteMapping, this); - m_serviceReadAllMappings = m_n.advertiseService("base/read_all_mappings", &BaseServices::ReadAllMappings, this); - m_serviceCreateMap = m_n.advertiseService("base/create_map", &BaseServices::CreateMap, this); - m_serviceReadMap = m_n.advertiseService("base/read_map", &BaseServices::ReadMap, this); - m_serviceUpdateMap = m_n.advertiseService("base/update_map", &BaseServices::UpdateMap, this); - m_serviceDeleteMap = m_n.advertiseService("base/delete_map", &BaseServices::DeleteMap, this); - m_serviceReadAllMaps = m_n.advertiseService("base/read_all_maps", &BaseServices::ReadAllMaps, this); - m_serviceActivateMap = m_n.advertiseService("base/activate_map", &BaseServices::ActivateMap, this); - m_serviceCreateAction = m_n.advertiseService("base/create_action", &BaseServices::CreateAction, this); - m_serviceReadAction = m_n.advertiseService("base/read_action", &BaseServices::ReadAction, this); - m_serviceReadAllActions = m_n.advertiseService("base/read_all_actions", &BaseServices::ReadAllActions, this); - m_serviceDeleteAction = m_n.advertiseService("base/delete_action", &BaseServices::DeleteAction, this); - m_serviceUpdateAction = m_n.advertiseService("base/update_action", &BaseServices::UpdateAction, this); - m_serviceExecuteActionFromReference = m_n.advertiseService("base/execute_action_from_reference", &BaseServices::ExecuteActionFromReference, this); - m_serviceExecuteAction = m_n.advertiseService("base/execute_action", &BaseServices::ExecuteAction, this); - m_servicePauseAction = m_n.advertiseService("base/pause_action", &BaseServices::PauseAction, this); - m_serviceStopAction = m_n.advertiseService("base/stop_action", &BaseServices::StopAction, this); - m_serviceResumeAction = m_n.advertiseService("base/resume_action", &BaseServices::ResumeAction, this); - m_serviceGetIPv4Configuration = m_n.advertiseService("base/get_i_pv4_configuration", &BaseServices::GetIPv4Configuration, this); - m_serviceSetIPv4Configuration = m_n.advertiseService("base/set_i_pv4_configuration", &BaseServices::SetIPv4Configuration, this); - m_serviceSetCommunicationInterfaceEnable = m_n.advertiseService("base/set_communication_interface_enable", &BaseServices::SetCommunicationInterfaceEnable, this); - m_serviceIsCommunicationInterfaceEnable = m_n.advertiseService("base/is_communication_interface_enable", &BaseServices::IsCommunicationInterfaceEnable, this); - m_serviceGetAvailableWifi = m_n.advertiseService("base/get_available_wifi", &BaseServices::GetAvailableWifi, this); - m_serviceGetWifiInformation = m_n.advertiseService("base/get_wifi_information", &BaseServices::GetWifiInformation, this); - m_serviceAddWifiConfiguration = m_n.advertiseService("base/add_wifi_configuration", &BaseServices::AddWifiConfiguration, this); - m_serviceDeleteWifiConfiguration = m_n.advertiseService("base/delete_wifi_configuration", &BaseServices::DeleteWifiConfiguration, this); - m_serviceGetAllConfiguredWifis = m_n.advertiseService("base/get_all_configured_wifis", &BaseServices::GetAllConfiguredWifis, this); - m_serviceConnectWifi = m_n.advertiseService("base/connect_wifi", &BaseServices::ConnectWifi, this); - m_serviceDisconnectWifi = m_n.advertiseService("base/disconnect_wifi", &BaseServices::DisconnectWifi, this); - m_serviceGetConnectedWifiInformation = m_n.advertiseService("base/get_connected_wifi_information", &BaseServices::GetConnectedWifiInformation, this); - m_serviceBase_Unsubscribe = m_n.advertiseService("base/unsubscribe", &BaseServices::Base_Unsubscribe, this); - m_serviceOnNotificationConfigurationChangeTopic = m_n.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseServices::OnNotificationConfigurationChangeTopic, this); - m_serviceOnNotificationMappingInfoTopic = m_n.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseServices::OnNotificationMappingInfoTopic, this); - m_serviceOnNotificationControlModeTopic = m_n.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseServices::OnNotificationControlModeTopic, this); - m_serviceOnNotificationOperatingModeTopic = m_n.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseServices::OnNotificationOperatingModeTopic, this); - m_serviceOnNotificationSequenceInfoTopic = m_n.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseServices::OnNotificationSequenceInfoTopic, this); - m_serviceOnNotificationProtectionZoneTopic = m_n.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseServices::OnNotificationProtectionZoneTopic, this); - m_serviceOnNotificationUserTopic = m_n.advertiseService("base/activate_publishing_of_user_topic", &BaseServices::OnNotificationUserTopic, this); - m_serviceOnNotificationControllerTopic = m_n.advertiseService("base/activate_publishing_of_controller_topic", &BaseServices::OnNotificationControllerTopic, this); - m_serviceOnNotificationActionTopic = m_n.advertiseService("base/activate_publishing_of_action_topic", &BaseServices::OnNotificationActionTopic, this); - m_serviceOnNotificationRobotEventTopic = m_n.advertiseService("base/activate_publishing_of_robot_event_topic", &BaseServices::OnNotificationRobotEventTopic, this); - m_servicePlayCartesianTrajectory = m_n.advertiseService("base/play_cartesian_trajectory", &BaseServices::PlayCartesianTrajectory, this); - m_servicePlayCartesianTrajectoryPosition = m_n.advertiseService("base/play_cartesian_trajectory_position", &BaseServices::PlayCartesianTrajectoryPosition, this); - m_servicePlayCartesianTrajectoryOrientation = m_n.advertiseService("base/play_cartesian_trajectory_orientation", &BaseServices::PlayCartesianTrajectoryOrientation, this); - m_serviceStop = m_n.advertiseService("base/stop", &BaseServices::Stop, this); - m_serviceGetMeasuredCartesianPose = m_n.advertiseService("base/get_measured_cartesian_pose", &BaseServices::GetMeasuredCartesianPose, this); - m_serviceSendWrenchCommand = m_n.advertiseService("base/send_wrench_command", &BaseServices::SendWrenchCommand, this); - m_serviceSendWrenchJoystickCommand = m_n.advertiseService("base/send_wrench_joystick_command", &BaseServices::SendWrenchJoystickCommand, this); - m_serviceSendTwistJoystickCommand = m_n.advertiseService("base/send_twist_joystick_command", &BaseServices::SendTwistJoystickCommand, this); - m_serviceSendTwistCommand = m_n.advertiseService("base/send_twist_command", &BaseServices::SendTwistCommand, this); - m_servicePlayJointTrajectory = m_n.advertiseService("base/play_joint_trajectory", &BaseServices::PlayJointTrajectory, this); - m_servicePlaySelectedJointTrajectory = m_n.advertiseService("base/play_selected_joint_trajectory", &BaseServices::PlaySelectedJointTrajectory, this); - m_serviceGetMeasuredJointAngles = m_n.advertiseService("base/get_measured_joint_angles", &BaseServices::GetMeasuredJointAngles, this); - m_serviceSendJointSpeedsCommand = m_n.advertiseService("base/send_joint_speeds_command", &BaseServices::SendJointSpeedsCommand, this); - m_serviceSendSelectedJointSpeedCommand = m_n.advertiseService("base/send_selected_joint_speed_command", &BaseServices::SendSelectedJointSpeedCommand, this); - m_serviceSendGripperCommand = m_n.advertiseService("base/send_gripper_command", &BaseServices::SendGripperCommand, this); - m_serviceGetMeasuredGripperMovement = m_n.advertiseService("base/get_measured_gripper_movement", &BaseServices::GetMeasuredGripperMovement, this); - m_serviceSetAdmittance = m_n.advertiseService("base/set_admittance", &BaseServices::SetAdmittance, this); - m_serviceSetOperatingMode = m_n.advertiseService("base/set_operating_mode", &BaseServices::SetOperatingMode, this); - m_serviceApplyEmergencyStop = m_n.advertiseService("base/apply_emergency_stop", &BaseServices::ApplyEmergencyStop, this); - m_serviceBase_ClearFaults = m_n.advertiseService("base/clear_faults", &BaseServices::Base_ClearFaults, this); - m_serviceBase_GetControlMode = m_n.advertiseService("base/get_control_mode", &BaseServices::Base_GetControlMode, this); - m_serviceGetOperatingMode = m_n.advertiseService("base/get_operating_mode", &BaseServices::GetOperatingMode, this); - m_serviceSetServoingMode = m_n.advertiseService("base/set_servoing_mode", &BaseServices::SetServoingMode, this); - m_serviceGetServoingMode = m_n.advertiseService("base/get_servoing_mode", &BaseServices::GetServoingMode, this); - m_serviceOnNotificationServoingModeTopic = m_n.advertiseService("base/activate_publishing_of_servoing_mode_topic", &BaseServices::OnNotificationServoingModeTopic, this); - m_serviceRestoreFactorySettings = m_n.advertiseService("base/restore_factory_settings", &BaseServices::RestoreFactorySettings, this); - m_serviceOnNotificationFactoryTopic = m_n.advertiseService("base/activate_publishing_of_factory_topic", &BaseServices::OnNotificationFactoryTopic, this); - m_serviceGetAllConnectedControllers = m_n.advertiseService("base/get_all_connected_controllers", &BaseServices::GetAllConnectedControllers, this); - m_serviceGetControllerState = m_n.advertiseService("base/get_controller_state", &BaseServices::GetControllerState, this); - m_serviceGetActuatorCount = m_n.advertiseService("base/get_actuator_count", &BaseServices::GetActuatorCount, this); - m_serviceStartWifiScan = m_n.advertiseService("base/start_wifi_scan", &BaseServices::StartWifiScan, this); - m_serviceGetConfiguredWifi = m_n.advertiseService("base/get_configured_wifi", &BaseServices::GetConfiguredWifi, this); - m_serviceOnNotificationNetworkTopic = m_n.advertiseService("base/activate_publishing_of_network_topic", &BaseServices::OnNotificationNetworkTopic, this); - m_serviceGetArmState = m_n.advertiseService("base/get_arm_state", &BaseServices::GetArmState, this); - m_serviceOnNotificationArmStateTopic = m_n.advertiseService("base/activate_publishing_of_arm_state_topic", &BaseServices::OnNotificationArmStateTopic, this); - m_serviceGetIPv4Information = m_n.advertiseService("base/get_i_pv4_information", &BaseServices::GetIPv4Information, this); - m_serviceSetWifiCountryCode = m_n.advertiseService("base/set_wifi_country_code", &BaseServices::SetWifiCountryCode, this); - m_serviceGetWifiCountryCode = m_n.advertiseService("base/get_wifi_country_code", &BaseServices::GetWifiCountryCode, this); - m_serviceBase_SetCapSenseConfig = m_n.advertiseService("base/set_cap_sense_config", &BaseServices::Base_SetCapSenseConfig, this); - m_serviceBase_GetCapSenseConfig = m_n.advertiseService("base/get_cap_sense_config", &BaseServices::Base_GetCapSenseConfig, this); - m_serviceGetAllJointsSpeedHardLimitation = m_n.advertiseService("base/get_all_joints_speed_hard_limitation", &BaseServices::GetAllJointsSpeedHardLimitation, this); - m_serviceGetAllJointsTorqueHardLimitation = m_n.advertiseService("base/get_all_joints_torque_hard_limitation", &BaseServices::GetAllJointsTorqueHardLimitation, this); - m_serviceGetTwistHardLimitation = m_n.advertiseService("base/get_twist_hard_limitation", &BaseServices::GetTwistHardLimitation, this); - m_serviceGetWrenchHardLimitation = m_n.advertiseService("base/get_wrench_hard_limitation", &BaseServices::GetWrenchHardLimitation, this); - m_serviceSendJointSpeedsJoystickCommand = m_n.advertiseService("base/send_joint_speeds_joystick_command", &BaseServices::SendJointSpeedsJoystickCommand, this); - m_serviceSendSelectedJointSpeedJoystickCommand = m_n.advertiseService("base/send_selected_joint_speed_joystick_command", &BaseServices::SendSelectedJointSpeedJoystickCommand, this); - m_serviceEnableBridge = m_n.advertiseService("base/enable_bridge", &BaseServices::EnableBridge, this); - m_serviceDisableBridge = m_n.advertiseService("base/disable_bridge", &BaseServices::DisableBridge, this); - m_serviceGetBridgeList = m_n.advertiseService("base/get_bridge_list", &BaseServices::GetBridgeList, this); - m_serviceGetBridgeConfig = m_n.advertiseService("base/get_bridge_config", &BaseServices::GetBridgeConfig, this); - m_servicePlayPreComputedJointTrajectory = m_n.advertiseService("base/play_pre_computed_joint_trajectory", &BaseServices::PlayPreComputedJointTrajectory, this); - m_serviceGetProductConfiguration = m_n.advertiseService("base/get_product_configuration", &BaseServices::GetProductConfiguration, this); - m_serviceUpdateEndEffectorTypeConfiguration = m_n.advertiseService("base/update_end_effector_type_configuration", &BaseServices::UpdateEndEffectorTypeConfiguration, this); - m_serviceRestoreFactoryProductConfiguration = m_n.advertiseService("base/restore_factory_product_configuration", &BaseServices::RestoreFactoryProductConfiguration, this); - m_serviceGetTrajectoryErrorReport = m_n.advertiseService("base/get_trajectory_error_report", &BaseServices::GetTrajectoryErrorReport, this); - m_serviceGetAllJointsSpeedSoftLimitation = m_n.advertiseService("base/get_all_joints_speed_soft_limitation", &BaseServices::GetAllJointsSpeedSoftLimitation, this); - m_serviceGetAllJointsTorqueSoftLimitation = m_n.advertiseService("base/get_all_joints_torque_soft_limitation", &BaseServices::GetAllJointsTorqueSoftLimitation, this); - m_serviceGetTwistSoftLimitation = m_n.advertiseService("base/get_twist_soft_limitation", &BaseServices::GetTwistSoftLimitation, this); - m_serviceGetWrenchSoftLimitation = m_n.advertiseService("base/get_wrench_soft_limitation", &BaseServices::GetWrenchSoftLimitation, this); - m_serviceSetControllerConfigurationMode = m_n.advertiseService("base/set_controller_configuration_mode", &BaseServices::SetControllerConfigurationMode, this); - m_serviceGetControllerConfigurationMode = m_n.advertiseService("base/get_controller_configuration_mode", &BaseServices::GetControllerConfigurationMode, this); - m_serviceStartTeaching = m_n.advertiseService("base/start_teaching", &BaseServices::StartTeaching, this); - m_serviceStopTeaching = m_n.advertiseService("base/stop_teaching", &BaseServices::StopTeaching, this); - m_serviceAddSequenceTasks = m_n.advertiseService("base/add_sequence_tasks", &BaseServices::AddSequenceTasks, this); - m_serviceUpdateSequenceTask = m_n.advertiseService("base/update_sequence_task", &BaseServices::UpdateSequenceTask, this); - m_serviceSwapSequenceTasks = m_n.advertiseService("base/swap_sequence_tasks", &BaseServices::SwapSequenceTasks, this); - m_serviceReadSequenceTask = m_n.advertiseService("base/read_sequence_task", &BaseServices::ReadSequenceTask, this); - m_serviceReadAllSequenceTasks = m_n.advertiseService("base/read_all_sequence_tasks", &BaseServices::ReadAllSequenceTasks, this); - m_serviceDeleteSequenceTask = m_n.advertiseService("base/delete_sequence_task", &BaseServices::DeleteSequenceTask, this); - m_serviceDeleteAllSequenceTasks = m_n.advertiseService("base/delete_all_sequence_tasks", &BaseServices::DeleteAllSequenceTasks, this); - m_serviceTakeSnapshot = m_n.advertiseService("base/take_snapshot", &BaseServices::TakeSnapshot, this); - m_serviceGetFirmwareBundleVersions = m_n.advertiseService("base/get_firmware_bundle_versions", &BaseServices::GetFirmwareBundleVersions, this); - m_serviceMoveSequenceTask = m_n.advertiseService("base/move_sequence_task", &BaseServices::MoveSequenceTask, this); - m_serviceDuplicateMapping = m_n.advertiseService("base/duplicate_mapping", &BaseServices::DuplicateMapping, this); - m_serviceDuplicateMap = m_n.advertiseService("base/duplicate_map", &BaseServices::DuplicateMap, this); - m_serviceSetControllerConfiguration = m_n.advertiseService("base/set_controller_configuration", &BaseServices::SetControllerConfiguration, this); - m_serviceGetControllerConfiguration = m_n.advertiseService("base/get_controller_configuration", &BaseServices::GetControllerConfiguration, this); - m_serviceGetAllControllerConfigurations = m_n.advertiseService("base/get_all_controller_configurations", &BaseServices::GetAllControllerConfigurations, this); -} - -bool BaseServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) + m_serviceSetDeviceID = m_node_handle.advertiseService("base/set_device_id", &BaseRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("base/set_api_options", &BaseRobotServices::SetApiOptions, this); + + m_serviceCreateUserProfile = m_node_handle.advertiseService("base/create_user_profile", &BaseRobotServices::CreateUserProfile, this); + m_serviceUpdateUserProfile = m_node_handle.advertiseService("base/update_user_profile", &BaseRobotServices::UpdateUserProfile, this); + m_serviceReadUserProfile = m_node_handle.advertiseService("base/read_user_profile", &BaseRobotServices::ReadUserProfile, this); + m_serviceDeleteUserProfile = m_node_handle.advertiseService("base/delete_user_profile", &BaseRobotServices::DeleteUserProfile, this); + m_serviceReadAllUserProfiles = m_node_handle.advertiseService("base/read_all_user_profiles", &BaseRobotServices::ReadAllUserProfiles, this); + m_serviceReadAllUsers = m_node_handle.advertiseService("base/read_all_users", &BaseRobotServices::ReadAllUsers, this); + m_serviceChangePassword = m_node_handle.advertiseService("base/change_password", &BaseRobotServices::ChangePassword, this); + m_serviceCreateSequence = m_node_handle.advertiseService("base/create_sequence", &BaseRobotServices::CreateSequence, this); + m_serviceUpdateSequence = m_node_handle.advertiseService("base/update_sequence", &BaseRobotServices::UpdateSequence, this); + m_serviceReadSequence = m_node_handle.advertiseService("base/read_sequence", &BaseRobotServices::ReadSequence, this); + m_serviceDeleteSequence = m_node_handle.advertiseService("base/delete_sequence", &BaseRobotServices::DeleteSequence, this); + m_serviceReadAllSequences = m_node_handle.advertiseService("base/read_all_sequences", &BaseRobotServices::ReadAllSequences, this); + m_servicePlaySequence = m_node_handle.advertiseService("base/play_sequence", &BaseRobotServices::PlaySequence, this); + m_servicePlayAdvancedSequence = m_node_handle.advertiseService("base/play_advanced_sequence", &BaseRobotServices::PlayAdvancedSequence, this); + m_serviceStopSequence = m_node_handle.advertiseService("base/stop_sequence", &BaseRobotServices::StopSequence, this); + m_servicePauseSequence = m_node_handle.advertiseService("base/pause_sequence", &BaseRobotServices::PauseSequence, this); + m_serviceResumeSequence = m_node_handle.advertiseService("base/resume_sequence", &BaseRobotServices::ResumeSequence, this); + m_serviceCreateProtectionZone = m_node_handle.advertiseService("base/create_protection_zone", &BaseRobotServices::CreateProtectionZone, this); + m_serviceUpdateProtectionZone = m_node_handle.advertiseService("base/update_protection_zone", &BaseRobotServices::UpdateProtectionZone, this); + m_serviceReadProtectionZone = m_node_handle.advertiseService("base/read_protection_zone", &BaseRobotServices::ReadProtectionZone, this); + m_serviceDeleteProtectionZone = m_node_handle.advertiseService("base/delete_protection_zone", &BaseRobotServices::DeleteProtectionZone, this); + m_serviceReadAllProtectionZones = m_node_handle.advertiseService("base/read_all_protection_zones", &BaseRobotServices::ReadAllProtectionZones, this); + m_serviceCreateMapping = m_node_handle.advertiseService("base/create_mapping", &BaseRobotServices::CreateMapping, this); + m_serviceReadMapping = m_node_handle.advertiseService("base/read_mapping", &BaseRobotServices::ReadMapping, this); + m_serviceUpdateMapping = m_node_handle.advertiseService("base/update_mapping", &BaseRobotServices::UpdateMapping, this); + m_serviceDeleteMapping = m_node_handle.advertiseService("base/delete_mapping", &BaseRobotServices::DeleteMapping, this); + m_serviceReadAllMappings = m_node_handle.advertiseService("base/read_all_mappings", &BaseRobotServices::ReadAllMappings, this); + m_serviceCreateMap = m_node_handle.advertiseService("base/create_map", &BaseRobotServices::CreateMap, this); + m_serviceReadMap = m_node_handle.advertiseService("base/read_map", &BaseRobotServices::ReadMap, this); + m_serviceUpdateMap = m_node_handle.advertiseService("base/update_map", &BaseRobotServices::UpdateMap, this); + m_serviceDeleteMap = m_node_handle.advertiseService("base/delete_map", &BaseRobotServices::DeleteMap, this); + m_serviceReadAllMaps = m_node_handle.advertiseService("base/read_all_maps", &BaseRobotServices::ReadAllMaps, this); + m_serviceActivateMap = m_node_handle.advertiseService("base/activate_map", &BaseRobotServices::ActivateMap, this); + m_serviceCreateAction = m_node_handle.advertiseService("base/create_action", &BaseRobotServices::CreateAction, this); + m_serviceReadAction = m_node_handle.advertiseService("base/read_action", &BaseRobotServices::ReadAction, this); + m_serviceReadAllActions = m_node_handle.advertiseService("base/read_all_actions", &BaseRobotServices::ReadAllActions, this); + m_serviceDeleteAction = m_node_handle.advertiseService("base/delete_action", &BaseRobotServices::DeleteAction, this); + m_serviceUpdateAction = m_node_handle.advertiseService("base/update_action", &BaseRobotServices::UpdateAction, this); + m_serviceExecuteActionFromReference = m_node_handle.advertiseService("base/execute_action_from_reference", &BaseRobotServices::ExecuteActionFromReference, this); + m_serviceExecuteAction = m_node_handle.advertiseService("base/execute_action", &BaseRobotServices::ExecuteAction, this); + m_servicePauseAction = m_node_handle.advertiseService("base/pause_action", &BaseRobotServices::PauseAction, this); + m_serviceStopAction = m_node_handle.advertiseService("base/stop_action", &BaseRobotServices::StopAction, this); + m_serviceResumeAction = m_node_handle.advertiseService("base/resume_action", &BaseRobotServices::ResumeAction, this); + m_serviceGetIPv4Configuration = m_node_handle.advertiseService("base/get_i_pv4_configuration", &BaseRobotServices::GetIPv4Configuration, this); + m_serviceSetIPv4Configuration = m_node_handle.advertiseService("base/set_i_pv4_configuration", &BaseRobotServices::SetIPv4Configuration, this); + m_serviceSetCommunicationInterfaceEnable = m_node_handle.advertiseService("base/set_communication_interface_enable", &BaseRobotServices::SetCommunicationInterfaceEnable, this); + m_serviceIsCommunicationInterfaceEnable = m_node_handle.advertiseService("base/is_communication_interface_enable", &BaseRobotServices::IsCommunicationInterfaceEnable, this); + m_serviceGetAvailableWifi = m_node_handle.advertiseService("base/get_available_wifi", &BaseRobotServices::GetAvailableWifi, this); + m_serviceGetWifiInformation = m_node_handle.advertiseService("base/get_wifi_information", &BaseRobotServices::GetWifiInformation, this); + m_serviceAddWifiConfiguration = m_node_handle.advertiseService("base/add_wifi_configuration", &BaseRobotServices::AddWifiConfiguration, this); + m_serviceDeleteWifiConfiguration = m_node_handle.advertiseService("base/delete_wifi_configuration", &BaseRobotServices::DeleteWifiConfiguration, this); + m_serviceGetAllConfiguredWifis = m_node_handle.advertiseService("base/get_all_configured_wifis", &BaseRobotServices::GetAllConfiguredWifis, this); + m_serviceConnectWifi = m_node_handle.advertiseService("base/connect_wifi", &BaseRobotServices::ConnectWifi, this); + m_serviceDisconnectWifi = m_node_handle.advertiseService("base/disconnect_wifi", &BaseRobotServices::DisconnectWifi, this); + m_serviceGetConnectedWifiInformation = m_node_handle.advertiseService("base/get_connected_wifi_information", &BaseRobotServices::GetConnectedWifiInformation, this); + m_serviceBase_Unsubscribe = m_node_handle.advertiseService("base/unsubscribe", &BaseRobotServices::Base_Unsubscribe, this); + m_serviceOnNotificationConfigurationChangeTopic = m_node_handle.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseRobotServices::OnNotificationConfigurationChangeTopic, this); + m_serviceOnNotificationMappingInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseRobotServices::OnNotificationMappingInfoTopic, this); + m_serviceOnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseRobotServices::OnNotificationControlModeTopic, this); + m_serviceOnNotificationOperatingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseRobotServices::OnNotificationOperatingModeTopic, this); + m_serviceOnNotificationSequenceInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseRobotServices::OnNotificationSequenceInfoTopic, this); + m_serviceOnNotificationProtectionZoneTopic = m_node_handle.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseRobotServices::OnNotificationProtectionZoneTopic, this); + m_serviceOnNotificationUserTopic = m_node_handle.advertiseService("base/activate_publishing_of_user_topic", &BaseRobotServices::OnNotificationUserTopic, this); + m_serviceOnNotificationControllerTopic = m_node_handle.advertiseService("base/activate_publishing_of_controller_topic", &BaseRobotServices::OnNotificationControllerTopic, this); + m_serviceOnNotificationActionTopic = m_node_handle.advertiseService("base/activate_publishing_of_action_topic", &BaseRobotServices::OnNotificationActionTopic, this); + m_serviceOnNotificationRobotEventTopic = m_node_handle.advertiseService("base/activate_publishing_of_robot_event_topic", &BaseRobotServices::OnNotificationRobotEventTopic, this); + m_servicePlayCartesianTrajectory = m_node_handle.advertiseService("base/play_cartesian_trajectory", &BaseRobotServices::PlayCartesianTrajectory, this); + m_servicePlayCartesianTrajectoryPosition = m_node_handle.advertiseService("base/play_cartesian_trajectory_position", &BaseRobotServices::PlayCartesianTrajectoryPosition, this); + m_servicePlayCartesianTrajectoryOrientation = m_node_handle.advertiseService("base/play_cartesian_trajectory_orientation", &BaseRobotServices::PlayCartesianTrajectoryOrientation, this); + m_serviceStop = m_node_handle.advertiseService("base/stop", &BaseRobotServices::Stop, this); + m_serviceGetMeasuredCartesianPose = m_node_handle.advertiseService("base/get_measured_cartesian_pose", &BaseRobotServices::GetMeasuredCartesianPose, this); + m_serviceSendWrenchCommand = m_node_handle.advertiseService("base/send_wrench_command", &BaseRobotServices::SendWrenchCommand, this); + m_serviceSendWrenchJoystickCommand = m_node_handle.advertiseService("base/send_wrench_joystick_command", &BaseRobotServices::SendWrenchJoystickCommand, this); + m_serviceSendTwistJoystickCommand = m_node_handle.advertiseService("base/send_twist_joystick_command", &BaseRobotServices::SendTwistJoystickCommand, this); + m_serviceSendTwistCommand = m_node_handle.advertiseService("base/send_twist_command", &BaseRobotServices::SendTwistCommand, this); + m_servicePlayJointTrajectory = m_node_handle.advertiseService("base/play_joint_trajectory", &BaseRobotServices::PlayJointTrajectory, this); + m_servicePlaySelectedJointTrajectory = m_node_handle.advertiseService("base/play_selected_joint_trajectory", &BaseRobotServices::PlaySelectedJointTrajectory, this); + m_serviceGetMeasuredJointAngles = m_node_handle.advertiseService("base/get_measured_joint_angles", &BaseRobotServices::GetMeasuredJointAngles, this); + m_serviceSendJointSpeedsCommand = m_node_handle.advertiseService("base/send_joint_speeds_command", &BaseRobotServices::SendJointSpeedsCommand, this); + m_serviceSendSelectedJointSpeedCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_command", &BaseRobotServices::SendSelectedJointSpeedCommand, this); + m_serviceSendGripperCommand = m_node_handle.advertiseService("base/send_gripper_command", &BaseRobotServices::SendGripperCommand, this); + m_serviceGetMeasuredGripperMovement = m_node_handle.advertiseService("base/get_measured_gripper_movement", &BaseRobotServices::GetMeasuredGripperMovement, this); + m_serviceSetAdmittance = m_node_handle.advertiseService("base/set_admittance", &BaseRobotServices::SetAdmittance, this); + m_serviceSetOperatingMode = m_node_handle.advertiseService("base/set_operating_mode", &BaseRobotServices::SetOperatingMode, this); + m_serviceApplyEmergencyStop = m_node_handle.advertiseService("base/apply_emergency_stop", &BaseRobotServices::ApplyEmergencyStop, this); + m_serviceBase_ClearFaults = m_node_handle.advertiseService("base/clear_faults", &BaseRobotServices::Base_ClearFaults, this); + m_serviceBase_GetControlMode = m_node_handle.advertiseService("base/get_control_mode", &BaseRobotServices::Base_GetControlMode, this); + m_serviceGetOperatingMode = m_node_handle.advertiseService("base/get_operating_mode", &BaseRobotServices::GetOperatingMode, this); + m_serviceSetServoingMode = m_node_handle.advertiseService("base/set_servoing_mode", &BaseRobotServices::SetServoingMode, this); + m_serviceGetServoingMode = m_node_handle.advertiseService("base/get_servoing_mode", &BaseRobotServices::GetServoingMode, this); + m_serviceOnNotificationServoingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_servoing_mode_topic", &BaseRobotServices::OnNotificationServoingModeTopic, this); + m_serviceRestoreFactorySettings = m_node_handle.advertiseService("base/restore_factory_settings", &BaseRobotServices::RestoreFactorySettings, this); + m_serviceOnNotificationFactoryTopic = m_node_handle.advertiseService("base/activate_publishing_of_factory_topic", &BaseRobotServices::OnNotificationFactoryTopic, this); + m_serviceGetAllConnectedControllers = m_node_handle.advertiseService("base/get_all_connected_controllers", &BaseRobotServices::GetAllConnectedControllers, this); + m_serviceGetControllerState = m_node_handle.advertiseService("base/get_controller_state", &BaseRobotServices::GetControllerState, this); + m_serviceGetActuatorCount = m_node_handle.advertiseService("base/get_actuator_count", &BaseRobotServices::GetActuatorCount, this); + m_serviceStartWifiScan = m_node_handle.advertiseService("base/start_wifi_scan", &BaseRobotServices::StartWifiScan, this); + m_serviceGetConfiguredWifi = m_node_handle.advertiseService("base/get_configured_wifi", &BaseRobotServices::GetConfiguredWifi, this); + m_serviceOnNotificationNetworkTopic = m_node_handle.advertiseService("base/activate_publishing_of_network_topic", &BaseRobotServices::OnNotificationNetworkTopic, this); + m_serviceGetArmState = m_node_handle.advertiseService("base/get_arm_state", &BaseRobotServices::GetArmState, this); + m_serviceOnNotificationArmStateTopic = m_node_handle.advertiseService("base/activate_publishing_of_arm_state_topic", &BaseRobotServices::OnNotificationArmStateTopic, this); + m_serviceGetIPv4Information = m_node_handle.advertiseService("base/get_i_pv4_information", &BaseRobotServices::GetIPv4Information, this); + m_serviceSetWifiCountryCode = m_node_handle.advertiseService("base/set_wifi_country_code", &BaseRobotServices::SetWifiCountryCode, this); + m_serviceGetWifiCountryCode = m_node_handle.advertiseService("base/get_wifi_country_code", &BaseRobotServices::GetWifiCountryCode, this); + m_serviceBase_SetCapSenseConfig = m_node_handle.advertiseService("base/set_cap_sense_config", &BaseRobotServices::Base_SetCapSenseConfig, this); + m_serviceBase_GetCapSenseConfig = m_node_handle.advertiseService("base/get_cap_sense_config", &BaseRobotServices::Base_GetCapSenseConfig, this); + m_serviceGetAllJointsSpeedHardLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_hard_limitation", &BaseRobotServices::GetAllJointsSpeedHardLimitation, this); + m_serviceGetAllJointsTorqueHardLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_hard_limitation", &BaseRobotServices::GetAllJointsTorqueHardLimitation, this); + m_serviceGetTwistHardLimitation = m_node_handle.advertiseService("base/get_twist_hard_limitation", &BaseRobotServices::GetTwistHardLimitation, this); + m_serviceGetWrenchHardLimitation = m_node_handle.advertiseService("base/get_wrench_hard_limitation", &BaseRobotServices::GetWrenchHardLimitation, this); + m_serviceSendJointSpeedsJoystickCommand = m_node_handle.advertiseService("base/send_joint_speeds_joystick_command", &BaseRobotServices::SendJointSpeedsJoystickCommand, this); + m_serviceSendSelectedJointSpeedJoystickCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_joystick_command", &BaseRobotServices::SendSelectedJointSpeedJoystickCommand, this); + m_serviceEnableBridge = m_node_handle.advertiseService("base/enable_bridge", &BaseRobotServices::EnableBridge, this); + m_serviceDisableBridge = m_node_handle.advertiseService("base/disable_bridge", &BaseRobotServices::DisableBridge, this); + m_serviceGetBridgeList = m_node_handle.advertiseService("base/get_bridge_list", &BaseRobotServices::GetBridgeList, this); + m_serviceGetBridgeConfig = m_node_handle.advertiseService("base/get_bridge_config", &BaseRobotServices::GetBridgeConfig, this); + m_servicePlayPreComputedJointTrajectory = m_node_handle.advertiseService("base/play_pre_computed_joint_trajectory", &BaseRobotServices::PlayPreComputedJointTrajectory, this); + m_serviceGetProductConfiguration = m_node_handle.advertiseService("base/get_product_configuration", &BaseRobotServices::GetProductConfiguration, this); + m_serviceUpdateEndEffectorTypeConfiguration = m_node_handle.advertiseService("base/update_end_effector_type_configuration", &BaseRobotServices::UpdateEndEffectorTypeConfiguration, this); + m_serviceRestoreFactoryProductConfiguration = m_node_handle.advertiseService("base/restore_factory_product_configuration", &BaseRobotServices::RestoreFactoryProductConfiguration, this); + m_serviceGetTrajectoryErrorReport = m_node_handle.advertiseService("base/get_trajectory_error_report", &BaseRobotServices::GetTrajectoryErrorReport, this); + m_serviceGetAllJointsSpeedSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_soft_limitation", &BaseRobotServices::GetAllJointsSpeedSoftLimitation, this); + m_serviceGetAllJointsTorqueSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_soft_limitation", &BaseRobotServices::GetAllJointsTorqueSoftLimitation, this); + m_serviceGetTwistSoftLimitation = m_node_handle.advertiseService("base/get_twist_soft_limitation", &BaseRobotServices::GetTwistSoftLimitation, this); + m_serviceGetWrenchSoftLimitation = m_node_handle.advertiseService("base/get_wrench_soft_limitation", &BaseRobotServices::GetWrenchSoftLimitation, this); + m_serviceSetControllerConfigurationMode = m_node_handle.advertiseService("base/set_controller_configuration_mode", &BaseRobotServices::SetControllerConfigurationMode, this); + m_serviceGetControllerConfigurationMode = m_node_handle.advertiseService("base/get_controller_configuration_mode", &BaseRobotServices::GetControllerConfigurationMode, this); + m_serviceStartTeaching = m_node_handle.advertiseService("base/start_teaching", &BaseRobotServices::StartTeaching, this); + m_serviceStopTeaching = m_node_handle.advertiseService("base/stop_teaching", &BaseRobotServices::StopTeaching, this); + m_serviceAddSequenceTasks = m_node_handle.advertiseService("base/add_sequence_tasks", &BaseRobotServices::AddSequenceTasks, this); + m_serviceUpdateSequenceTask = m_node_handle.advertiseService("base/update_sequence_task", &BaseRobotServices::UpdateSequenceTask, this); + m_serviceSwapSequenceTasks = m_node_handle.advertiseService("base/swap_sequence_tasks", &BaseRobotServices::SwapSequenceTasks, this); + m_serviceReadSequenceTask = m_node_handle.advertiseService("base/read_sequence_task", &BaseRobotServices::ReadSequenceTask, this); + m_serviceReadAllSequenceTasks = m_node_handle.advertiseService("base/read_all_sequence_tasks", &BaseRobotServices::ReadAllSequenceTasks, this); + m_serviceDeleteSequenceTask = m_node_handle.advertiseService("base/delete_sequence_task", &BaseRobotServices::DeleteSequenceTask, this); + m_serviceDeleteAllSequenceTasks = m_node_handle.advertiseService("base/delete_all_sequence_tasks", &BaseRobotServices::DeleteAllSequenceTasks, this); + m_serviceTakeSnapshot = m_node_handle.advertiseService("base/take_snapshot", &BaseRobotServices::TakeSnapshot, this); + m_serviceGetFirmwareBundleVersions = m_node_handle.advertiseService("base/get_firmware_bundle_versions", &BaseRobotServices::GetFirmwareBundleVersions, this); + m_serviceMoveSequenceTask = m_node_handle.advertiseService("base/move_sequence_task", &BaseRobotServices::MoveSequenceTask, this); + m_serviceDuplicateMapping = m_node_handle.advertiseService("base/duplicate_mapping", &BaseRobotServices::DuplicateMapping, this); + m_serviceDuplicateMap = m_node_handle.advertiseService("base/duplicate_map", &BaseRobotServices::DuplicateMap, this); + m_serviceSetControllerConfiguration = m_node_handle.advertiseService("base/set_controller_configuration", &BaseRobotServices::SetControllerConfiguration, this); + m_serviceGetControllerConfiguration = m_node_handle.advertiseService("base/get_controller_configuration", &BaseRobotServices::GetControllerConfiguration, this); + m_serviceGetAllControllerConfigurations = m_node_handle.advertiseService("base/get_all_controller_configurations", &BaseRobotServices::GetAllControllerConfigurations, this); +} + +bool BaseRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool BaseServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool BaseRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -243,7 +243,7 @@ bool BaseServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, ko } -bool BaseServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) +bool BaseRobotServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) { Kinova::Api::Base::FullUserProfile input; @@ -278,7 +278,7 @@ bool BaseServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request return true; } -bool BaseServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) +bool BaseRobotServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) { Kinova::Api::Base::UserProfile input; @@ -310,7 +310,7 @@ bool BaseServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request return true; } -bool BaseServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) +bool BaseRobotServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) { Kinova::Api::Common::UserProfileHandle input; @@ -345,7 +345,7 @@ bool BaseServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req return true; } -bool BaseServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) +bool BaseRobotServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) { Kinova::Api::Common::UserProfileHandle input; @@ -377,7 +377,7 @@ bool BaseServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request return true; } -bool BaseServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) +bool BaseRobotServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) { Kinova::Api::Base::UserProfileList output; @@ -410,7 +410,7 @@ bool BaseServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Reque return true; } -bool BaseServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) +bool BaseRobotServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) { Kinova::Api::Base::UserList output; @@ -443,7 +443,7 @@ bool BaseServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kort return true; } -bool BaseServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) +bool BaseRobotServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) { Kinova::Api::Base::PasswordChange input; @@ -475,7 +475,7 @@ bool BaseServices::ChangePassword(kortex_driver::ChangePassword::Request &req, return true; } -bool BaseServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) +bool BaseRobotServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) { Kinova::Api::Base::Sequence input; @@ -510,7 +510,7 @@ bool BaseServices::CreateSequence(kortex_driver::CreateSequence::Request &req, return true; } -bool BaseServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) +bool BaseRobotServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) { Kinova::Api::Base::Sequence input; @@ -542,7 +542,7 @@ bool BaseServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, return true; } -bool BaseServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) +bool BaseRobotServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -577,7 +577,7 @@ bool BaseServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kort return true; } -bool BaseServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) +bool BaseRobotServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -609,7 +609,7 @@ bool BaseServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, return true; } -bool BaseServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) +bool BaseRobotServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) { Kinova::Api::Base::SequenceList output; @@ -642,7 +642,7 @@ bool BaseServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &r return true; } -bool BaseServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) +bool BaseRobotServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -674,7 +674,7 @@ bool BaseServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kort return true; } -bool BaseServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) +bool BaseRobotServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) { Kinova::Api::Base::AdvancedSequenceHandle input; @@ -706,7 +706,7 @@ bool BaseServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Req return true; } -bool BaseServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) +bool BaseRobotServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) { kortex_driver::KortexError result_error; @@ -736,7 +736,7 @@ bool BaseServices::StopSequence(kortex_driver::StopSequence::Request &req, kort return true; } -bool BaseServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) +bool BaseRobotServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) { kortex_driver::KortexError result_error; @@ -766,7 +766,7 @@ bool BaseServices::PauseSequence(kortex_driver::PauseSequence::Request &req, ko return true; } -bool BaseServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) +bool BaseRobotServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) { kortex_driver::KortexError result_error; @@ -796,7 +796,7 @@ bool BaseServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, return true; } -bool BaseServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) +bool BaseRobotServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZone input; @@ -831,7 +831,7 @@ bool BaseServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Req return true; } -bool BaseServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) +bool BaseRobotServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZone input; @@ -863,7 +863,7 @@ bool BaseServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Req return true; } -bool BaseServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) +bool BaseRobotServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZoneHandle input; @@ -898,7 +898,7 @@ bool BaseServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request return true; } -bool BaseServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) +bool BaseRobotServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZoneHandle input; @@ -930,7 +930,7 @@ bool BaseServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Req return true; } -bool BaseServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) +bool BaseRobotServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) { Kinova::Api::Base::ProtectionZoneList output; @@ -963,7 +963,7 @@ bool BaseServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones: return true; } -bool BaseServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) +bool BaseRobotServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) { Kinova::Api::Base::Mapping input; @@ -998,7 +998,7 @@ bool BaseServices::CreateMapping(kortex_driver::CreateMapping::Request &req, ko return true; } -bool BaseServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) +bool BaseRobotServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -1033,7 +1033,7 @@ bool BaseServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex return true; } -bool BaseServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) +bool BaseRobotServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) { Kinova::Api::Base::Mapping input; @@ -1065,7 +1065,7 @@ bool BaseServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, ko return true; } -bool BaseServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) +bool BaseRobotServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -1097,7 +1097,7 @@ bool BaseServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, ko return true; } -bool BaseServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) +bool BaseRobotServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) { Kinova::Api::Base::MappingList output; @@ -1130,7 +1130,7 @@ bool BaseServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req return true; } -bool BaseServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) +bool BaseRobotServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) { Kinova::Api::Base::Map input; @@ -1165,7 +1165,7 @@ bool BaseServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_dri return true; } -bool BaseServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) +bool BaseRobotServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) { Kinova::Api::Base::MapHandle input; @@ -1200,7 +1200,7 @@ bool BaseServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver: return true; } -bool BaseServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) +bool BaseRobotServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) { Kinova::Api::Base::Map input; @@ -1232,7 +1232,7 @@ bool BaseServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_dri return true; } -bool BaseServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) +bool BaseRobotServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) { Kinova::Api::Base::MapHandle input; @@ -1264,7 +1264,7 @@ bool BaseServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_dri return true; } -bool BaseServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) +bool BaseRobotServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -1299,7 +1299,7 @@ bool BaseServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex return true; } -bool BaseServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) +bool BaseRobotServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) { Kinova::Api::Base::ActivateMapHandle input; @@ -1331,7 +1331,7 @@ bool BaseServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex return true; } -bool BaseServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) +bool BaseRobotServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) { Kinova::Api::Base::Action input; @@ -1366,7 +1366,7 @@ bool BaseServices::CreateAction(kortex_driver::CreateAction::Request &req, kort return true; } -bool BaseServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) +bool BaseRobotServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) { Kinova::Api::Base::ActionHandle input; @@ -1401,7 +1401,7 @@ bool BaseServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_d return true; } -bool BaseServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) +bool BaseRobotServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) { Kinova::Api::Base::RequestedActionType input; @@ -1436,7 +1436,7 @@ bool BaseServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, return true; } -bool BaseServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) +bool BaseRobotServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) { Kinova::Api::Base::ActionHandle input; @@ -1468,7 +1468,7 @@ bool BaseServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kort return true; } -bool BaseServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) +bool BaseRobotServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) { Kinova::Api::Base::Action input; @@ -1500,7 +1500,7 @@ bool BaseServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kort return true; } -bool BaseServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) +bool BaseRobotServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) { Kinova::Api::Base::ActionHandle input; @@ -1532,7 +1532,7 @@ bool BaseServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromRe return true; } -bool BaseServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) +bool BaseRobotServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) { Kinova::Api::Base::Action input; @@ -1564,7 +1564,7 @@ bool BaseServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, ko return true; } -bool BaseServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) +bool BaseRobotServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) { kortex_driver::KortexError result_error; @@ -1594,7 +1594,7 @@ bool BaseServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex return true; } -bool BaseServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) +bool BaseRobotServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) { kortex_driver::KortexError result_error; @@ -1624,7 +1624,7 @@ bool BaseServices::StopAction(kortex_driver::StopAction::Request &req, kortex_d return true; } -bool BaseServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) +bool BaseRobotServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) { kortex_driver::KortexError result_error; @@ -1654,7 +1654,7 @@ bool BaseServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kort return true; } -bool BaseServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) +bool BaseRobotServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) { Kinova::Api::Base::NetworkHandle input; @@ -1689,7 +1689,7 @@ bool BaseServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Req return true; } -bool BaseServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) +bool BaseRobotServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) { Kinova::Api::Base::FullIPv4Configuration input; @@ -1721,7 +1721,7 @@ bool BaseServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Req return true; } -bool BaseServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) +bool BaseRobotServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) { Kinova::Api::Base::CommunicationInterfaceConfiguration input; @@ -1753,7 +1753,7 @@ bool BaseServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicati return true; } -bool BaseServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) +bool BaseRobotServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) { Kinova::Api::Base::NetworkHandle input; @@ -1788,7 +1788,7 @@ bool BaseServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunication return true; } -bool BaseServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) +bool BaseRobotServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) { Kinova::Api::Base::WifiInformationList output; @@ -1821,7 +1821,7 @@ bool BaseServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &r return true; } -bool BaseServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) +bool BaseRobotServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) { Kinova::Api::Base::Ssid input; @@ -1856,7 +1856,7 @@ bool BaseServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request return true; } -bool BaseServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) +bool BaseRobotServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) { Kinova::Api::Base::WifiConfiguration input; @@ -1888,7 +1888,7 @@ bool BaseServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Req return true; } -bool BaseServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) +bool BaseRobotServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) { Kinova::Api::Base::Ssid input; @@ -1920,7 +1920,7 @@ bool BaseServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguratio return true; } -bool BaseServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) +bool BaseRobotServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) { Kinova::Api::Base::WifiConfigurationList output; @@ -1953,7 +1953,7 @@ bool BaseServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::R return true; } -bool BaseServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) +bool BaseRobotServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) { Kinova::Api::Base::Ssid input; @@ -1985,7 +1985,7 @@ bool BaseServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex return true; } -bool BaseServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) +bool BaseRobotServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) { kortex_driver::KortexError result_error; @@ -2015,7 +2015,7 @@ bool BaseServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, return true; } -bool BaseServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) +bool BaseRobotServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) { Kinova::Api::Base::WifiInformation output; @@ -2048,7 +2048,7 @@ bool BaseServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiIn return true; } -bool BaseServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) +bool BaseRobotServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) { Kinova::Api::Common::NotificationHandle input; @@ -2080,7 +2080,7 @@ bool BaseServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &r return true; } -bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) +bool BaseRobotServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2094,7 +2094,7 @@ bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotif try { - std::function< void (Kinova::Api::Base::ConfigurationChangeNotification) > callback = std::bind(&BaseServices::cb_ConfigurationChangeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ConfigurationChangeNotification) > callback = std::bind(&BaseRobotServices::cb_ConfigurationChangeTopic, this, std::placeholders::_1); output = m_base->OnNotificationConfigurationChangeTopic(callback, input, m_current_device_id); m_is_activated_ConfigurationChangeTopic = true; } @@ -2119,14 +2119,14 @@ bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotif ToRosData(output, res.output); return true; } -void BaseServices::cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) +void BaseRobotServices::cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) { kortex_driver::ConfigurationChangeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ConfigurationChangeTopic.publish(ros_msg); } -bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) +bool BaseRobotServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2140,7 +2140,7 @@ bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationM try { - std::function< void (Kinova::Api::Base::MappingInfoNotification) > callback = std::bind(&BaseServices::cb_MappingInfoTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::MappingInfoNotification) > callback = std::bind(&BaseRobotServices::cb_MappingInfoTopic, this, std::placeholders::_1); output = m_base->OnNotificationMappingInfoTopic(callback, input, m_current_device_id); m_is_activated_MappingInfoTopic = true; } @@ -2165,14 +2165,14 @@ bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationM ToRosData(output, res.output); return true; } -void BaseServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) +void BaseRobotServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) { kortex_driver::MappingInfoNotification ros_msg; ToRosData(notif, ros_msg); m_pub_MappingInfoTopic.publish(ros_msg); } -bool BaseServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) +bool BaseRobotServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2186,7 +2186,7 @@ bool BaseServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationC try { - std::function< void (Kinova::Api::Base::ControlModeNotification) > callback = std::bind(&BaseServices::cb_ControlModeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ControlModeNotification) > callback = std::bind(&BaseRobotServices::cb_ControlModeTopic, this, std::placeholders::_1); output = m_base->OnNotificationControlModeTopic(callback, input, m_current_device_id); m_is_activated_ControlModeTopic = true; } @@ -2211,14 +2211,14 @@ bool BaseServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationC ToRosData(output, res.output); return true; } -void BaseServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) +void BaseRobotServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) { kortex_driver::ControlModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControlModeTopic.publish(ros_msg); } -bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) +bool BaseRobotServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2232,7 +2232,7 @@ bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificatio try { - std::function< void (Kinova::Api::Base::OperatingModeNotification) > callback = std::bind(&BaseServices::cb_OperatingModeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::OperatingModeNotification) > callback = std::bind(&BaseRobotServices::cb_OperatingModeTopic, this, std::placeholders::_1); output = m_base->OnNotificationOperatingModeTopic(callback, input, m_current_device_id); m_is_activated_OperatingModeTopic = true; } @@ -2257,14 +2257,14 @@ bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificatio ToRosData(output, res.output); return true; } -void BaseServices::cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) +void BaseRobotServices::cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) { kortex_driver::OperatingModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_OperatingModeTopic.publish(ros_msg); } -bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) +bool BaseRobotServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2278,7 +2278,7 @@ bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotification try { - std::function< void (Kinova::Api::Base::SequenceInfoNotification) > callback = std::bind(&BaseServices::cb_SequenceInfoTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::SequenceInfoNotification) > callback = std::bind(&BaseRobotServices::cb_SequenceInfoTopic, this, std::placeholders::_1); output = m_base->OnNotificationSequenceInfoTopic(callback, input, m_current_device_id); m_is_activated_SequenceInfoTopic = true; } @@ -2303,14 +2303,14 @@ bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotification ToRosData(output, res.output); return true; } -void BaseServices::cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) +void BaseRobotServices::cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) { kortex_driver::SequenceInfoNotification ros_msg; ToRosData(notif, ros_msg); m_pub_SequenceInfoTopic.publish(ros_msg); } -bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) +bool BaseRobotServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2324,7 +2324,7 @@ bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificati try { - std::function< void (Kinova::Api::Base::ProtectionZoneNotification) > callback = std::bind(&BaseServices::cb_ProtectionZoneTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ProtectionZoneNotification) > callback = std::bind(&BaseRobotServices::cb_ProtectionZoneTopic, this, std::placeholders::_1); output = m_base->OnNotificationProtectionZoneTopic(callback, input, m_current_device_id); m_is_activated_ProtectionZoneTopic = true; } @@ -2349,14 +2349,14 @@ bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificati ToRosData(output, res.output); return true; } -void BaseServices::cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) +void BaseRobotServices::cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) { kortex_driver::ProtectionZoneNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ProtectionZoneTopic.publish(ros_msg); } -bool BaseServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) +bool BaseRobotServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2370,7 +2370,7 @@ bool BaseServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopi try { - std::function< void (Kinova::Api::Base::UserNotification) > callback = std::bind(&BaseServices::cb_UserTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::UserNotification) > callback = std::bind(&BaseRobotServices::cb_UserTopic, this, std::placeholders::_1); output = m_base->OnNotificationUserTopic(callback, input, m_current_device_id); m_is_activated_UserTopic = true; } @@ -2395,14 +2395,14 @@ bool BaseServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopi ToRosData(output, res.output); return true; } -void BaseServices::cb_UserTopic(Kinova::Api::Base::UserNotification notif) +void BaseRobotServices::cb_UserTopic(Kinova::Api::Base::UserNotification notif) { kortex_driver::UserNotification ros_msg; ToRosData(notif, ros_msg); m_pub_UserTopic.publish(ros_msg); } -bool BaseServices::OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) +bool BaseRobotServices::OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2416,7 +2416,7 @@ bool BaseServices::OnNotificationControllerTopic(kortex_driver::OnNotificationCo try { - std::function< void (Kinova::Api::Base::ControllerNotification) > callback = std::bind(&BaseServices::cb_ControllerTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ControllerNotification) > callback = std::bind(&BaseRobotServices::cb_ControllerTopic, this, std::placeholders::_1); output = m_base->OnNotificationControllerTopic(callback, input, m_current_device_id); m_is_activated_ControllerTopic = true; } @@ -2441,14 +2441,14 @@ bool BaseServices::OnNotificationControllerTopic(kortex_driver::OnNotificationCo ToRosData(output, res.output); return true; } -void BaseServices::cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) +void BaseRobotServices::cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) { kortex_driver::ControllerNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControllerTopic.publish(ros_msg); } -bool BaseServices::OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) +bool BaseRobotServices::OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2462,7 +2462,7 @@ bool BaseServices::OnNotificationActionTopic(kortex_driver::OnNotificationAction try { - std::function< void (Kinova::Api::Base::ActionNotification) > callback = std::bind(&BaseServices::cb_ActionTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ActionNotification) > callback = std::bind(&BaseRobotServices::cb_ActionTopic, this, std::placeholders::_1); output = m_base->OnNotificationActionTopic(callback, input, m_current_device_id); m_is_activated_ActionTopic = true; } @@ -2487,14 +2487,14 @@ bool BaseServices::OnNotificationActionTopic(kortex_driver::OnNotificationAction ToRosData(output, res.output); return true; } -void BaseServices::cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) +void BaseRobotServices::cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) { kortex_driver::ActionNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ActionTopic.publish(ros_msg); } -bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) +bool BaseRobotServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2508,7 +2508,7 @@ bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRo try { - std::function< void (Kinova::Api::Base::RobotEventNotification) > callback = std::bind(&BaseServices::cb_RobotEventTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::RobotEventNotification) > callback = std::bind(&BaseRobotServices::cb_RobotEventTopic, this, std::placeholders::_1); output = m_base->OnNotificationRobotEventTopic(callback, input, m_current_device_id); m_is_activated_RobotEventTopic = true; } @@ -2533,14 +2533,14 @@ bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRo ToRosData(output, res.output); return true; } -void BaseServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) +void BaseRobotServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) { kortex_driver::RobotEventNotification ros_msg; ToRosData(notif, ros_msg); m_pub_RobotEventTopic.publish(ros_msg); } -bool BaseServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) +bool BaseRobotServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) { Kinova::Api::Base::ConstrainedPose input; @@ -2572,7 +2572,7 @@ bool BaseServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajector return true; } -bool BaseServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) +bool BaseRobotServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) { Kinova::Api::Base::ConstrainedPosition input; @@ -2604,7 +2604,7 @@ bool BaseServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianT return true; } -bool BaseServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) +bool BaseRobotServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) { Kinova::Api::Base::ConstrainedOrientation input; @@ -2636,7 +2636,7 @@ bool BaseServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesi return true; } -bool BaseServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) +bool BaseRobotServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) { kortex_driver::KortexError result_error; @@ -2666,7 +2666,7 @@ bool BaseServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop: return true; } -bool BaseServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) +bool BaseRobotServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) { Kinova::Api::Base::Pose output; @@ -2699,7 +2699,7 @@ bool BaseServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianP return true; } -bool BaseServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) +bool BaseRobotServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) { Kinova::Api::Base::WrenchCommand input; @@ -2731,7 +2731,7 @@ bool BaseServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request return true; } -bool BaseServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) +bool BaseRobotServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) { Kinova::Api::Base::WrenchCommand input; @@ -2763,7 +2763,7 @@ bool BaseServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCo return true; } -bool BaseServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) +bool BaseRobotServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) { Kinova::Api::Base::TwistCommand input; @@ -2795,7 +2795,7 @@ bool BaseServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickComm return true; } -bool BaseServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) +bool BaseRobotServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) { Kinova::Api::Base::TwistCommand input; @@ -2827,7 +2827,7 @@ bool BaseServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &r return true; } -bool BaseServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) +bool BaseRobotServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) { Kinova::Api::Base::ConstrainedJointAngles input; @@ -2859,7 +2859,7 @@ bool BaseServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Reque return true; } -bool BaseServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) +bool BaseRobotServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) { Kinova::Api::Base::ConstrainedJointAngle input; @@ -2891,7 +2891,7 @@ bool BaseServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointT return true; } -bool BaseServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) +bool BaseRobotServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) { Kinova::Api::Base::JointAngles output; @@ -2924,7 +2924,7 @@ bool BaseServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles: return true; } -bool BaseServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) +bool BaseRobotServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) { Kinova::Api::Base::JointSpeeds input; @@ -2956,7 +2956,7 @@ bool BaseServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand: return true; } -bool BaseServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) +bool BaseRobotServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) { Kinova::Api::Base::JointSpeed input; @@ -2988,7 +2988,7 @@ bool BaseServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJoin return true; } -bool BaseServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) +bool BaseRobotServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) { Kinova::Api::Base::GripperCommand input; @@ -3020,7 +3020,7 @@ bool BaseServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request return true; } -bool BaseServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) +bool BaseRobotServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) { Kinova::Api::Base::GripperRequest input; @@ -3055,7 +3055,7 @@ bool BaseServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperM return true; } -bool BaseServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) +bool BaseRobotServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) { Kinova::Api::Base::Admittance input; @@ -3087,7 +3087,7 @@ bool BaseServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, ko return true; } -bool BaseServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) +bool BaseRobotServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) { Kinova::Api::Base::OperatingModeInformation input; @@ -3119,7 +3119,7 @@ bool BaseServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &r return true; } -bool BaseServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) +bool BaseRobotServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) { kortex_driver::KortexError result_error; @@ -3149,7 +3149,7 @@ bool BaseServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request return true; } -bool BaseServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) +bool BaseRobotServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) { kortex_driver::KortexError result_error; @@ -3179,7 +3179,7 @@ bool BaseServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &r return true; } -bool BaseServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) +bool BaseRobotServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) { Kinova::Api::Base::ControlModeInformation output; @@ -3212,7 +3212,7 @@ bool BaseServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Reque return true; } -bool BaseServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) +bool BaseRobotServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) { Kinova::Api::Base::OperatingModeInformation output; @@ -3245,7 +3245,7 @@ bool BaseServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &r return true; } -bool BaseServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) +bool BaseRobotServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) { Kinova::Api::Base::ServoingModeInformation input; @@ -3277,7 +3277,7 @@ bool BaseServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req return true; } -bool BaseServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) +bool BaseRobotServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) { Kinova::Api::Base::ServoingModeInformation output; @@ -3310,7 +3310,7 @@ bool BaseServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req return true; } -bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) +bool BaseRobotServices::OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3324,7 +3324,7 @@ bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::OnNotification try { - std::function< void (Kinova::Api::Base::ServoingModeNotification) > callback = std::bind(&BaseServices::cb_ServoingModeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ServoingModeNotification) > callback = std::bind(&BaseRobotServices::cb_ServoingModeTopic, this, std::placeholders::_1); output = m_base->OnNotificationServoingModeTopic(callback, input, m_current_device_id); m_is_activated_ServoingModeTopic = true; } @@ -3349,14 +3349,14 @@ bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::OnNotification ToRosData(output, res.output); return true; } -void BaseServices::cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) +void BaseRobotServices::cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) { kortex_driver::ServoingModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ServoingModeTopic.publish(ros_msg); } -bool BaseServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) +bool BaseRobotServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) { kortex_driver::KortexError result_error; @@ -3386,7 +3386,7 @@ bool BaseServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings: return true; } -bool BaseServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) +bool BaseRobotServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3400,7 +3400,7 @@ bool BaseServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFacto try { - std::function< void (Kinova::Api::Base::FactoryNotification) > callback = std::bind(&BaseServices::cb_FactoryTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::FactoryNotification) > callback = std::bind(&BaseRobotServices::cb_FactoryTopic, this, std::placeholders::_1); output = m_base->OnNotificationFactoryTopic(callback, input, m_current_device_id); m_is_activated_FactoryTopic = true; } @@ -3425,14 +3425,14 @@ bool BaseServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFacto ToRosData(output, res.output); return true; } -void BaseServices::cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) +void BaseRobotServices::cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) { kortex_driver::FactoryNotification ros_msg; ToRosData(notif, ros_msg); m_pub_FactoryTopic.publish(ros_msg); } -bool BaseServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) +bool BaseRobotServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) { Kinova::Api::Base::ControllerList output; @@ -3465,7 +3465,7 @@ bool BaseServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedCont return true; } -bool BaseServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) +bool BaseRobotServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) { Kinova::Api::Base::ControllerHandle input; @@ -3500,7 +3500,7 @@ bool BaseServices::GetControllerState(kortex_driver::GetControllerState::Request return true; } -bool BaseServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) +bool BaseRobotServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) { Kinova::Api::Base::ActuatorInformation output; @@ -3533,7 +3533,7 @@ bool BaseServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &r return true; } -bool BaseServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) +bool BaseRobotServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) { kortex_driver::KortexError result_error; @@ -3563,7 +3563,7 @@ bool BaseServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, ko return true; } -bool BaseServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) +bool BaseRobotServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) { Kinova::Api::Base::Ssid input; @@ -3598,7 +3598,7 @@ bool BaseServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request return true; } -bool BaseServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) +bool BaseRobotServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3612,7 +3612,7 @@ bool BaseServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetwo try { - std::function< void (Kinova::Api::Base::NetworkNotification) > callback = std::bind(&BaseServices::cb_NetworkTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::NetworkNotification) > callback = std::bind(&BaseRobotServices::cb_NetworkTopic, this, std::placeholders::_1); output = m_base->OnNotificationNetworkTopic(callback, input, m_current_device_id); m_is_activated_NetworkTopic = true; } @@ -3637,14 +3637,14 @@ bool BaseServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetwo ToRosData(output, res.output); return true; } -void BaseServices::cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) +void BaseRobotServices::cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) { kortex_driver::NetworkNotification ros_msg; ToRosData(notif, ros_msg); m_pub_NetworkTopic.publish(ros_msg); } -bool BaseServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) +bool BaseRobotServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) { Kinova::Api::Base::ArmStateInformation output; @@ -3677,7 +3677,7 @@ bool BaseServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex return true; } -bool BaseServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) +bool BaseRobotServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3691,7 +3691,7 @@ bool BaseServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmS try { - std::function< void (Kinova::Api::Base::ArmStateNotification) > callback = std::bind(&BaseServices::cb_ArmStateTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ArmStateNotification) > callback = std::bind(&BaseRobotServices::cb_ArmStateTopic, this, std::placeholders::_1); output = m_base->OnNotificationArmStateTopic(callback, input, m_current_device_id); m_is_activated_ArmStateTopic = true; } @@ -3716,14 +3716,14 @@ bool BaseServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmS ToRosData(output, res.output); return true; } -void BaseServices::cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) +void BaseRobotServices::cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) { kortex_driver::ArmStateNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ArmStateTopic.publish(ros_msg); } -bool BaseServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) +bool BaseRobotServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) { Kinova::Api::Base::NetworkHandle input; @@ -3758,7 +3758,7 @@ bool BaseServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request return true; } -bool BaseServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) +bool BaseRobotServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) { Kinova::Api::Common::CountryCode input; @@ -3790,7 +3790,7 @@ bool BaseServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request return true; } -bool BaseServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) +bool BaseRobotServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) { Kinova::Api::Common::CountryCode output; @@ -3823,7 +3823,7 @@ bool BaseServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request return true; } -bool BaseServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) +bool BaseRobotServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) { Kinova::Api::Base::CapSenseConfig input; @@ -3855,7 +3855,7 @@ bool BaseServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig: return true; } -bool BaseServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) +bool BaseRobotServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) { Kinova::Api::Base::CapSenseConfig output; @@ -3888,9 +3888,9 @@ bool BaseServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig: return true; } -bool BaseServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) +bool BaseRobotServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_speed_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_speed_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -3922,9 +3922,9 @@ bool BaseServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSp return true; } -bool BaseServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) +bool BaseRobotServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_torque_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_torque_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -3956,9 +3956,9 @@ bool BaseServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsT return true; } -bool BaseServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) +bool BaseRobotServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) { - ROS_WARN("The base/get_twist_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_twist_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::TwistLimitation output; @@ -3990,9 +3990,9 @@ bool BaseServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation: return true; } -bool BaseServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) +bool BaseRobotServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) { - ROS_WARN("The base/get_wrench_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_wrench_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::WrenchLimitation output; @@ -4024,7 +4024,7 @@ bool BaseServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitatio return true; } -bool BaseServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) +bool BaseRobotServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) { Kinova::Api::Base::JointSpeeds input; @@ -4056,7 +4056,7 @@ bool BaseServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeeds return true; } -bool BaseServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) +bool BaseRobotServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) { Kinova::Api::Base::JointSpeed input; @@ -4088,7 +4088,7 @@ bool BaseServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSele return true; } -bool BaseServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) +bool BaseRobotServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) { Kinova::Api::Base::BridgeConfig input; @@ -4123,7 +4123,7 @@ bool BaseServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kort return true; } -bool BaseServices::DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) +bool BaseRobotServices::DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) { Kinova::Api::Base::BridgeIdentifier input; @@ -4158,7 +4158,7 @@ bool BaseServices::DisableBridge(kortex_driver::DisableBridge::Request &req, ko return true; } -bool BaseServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) +bool BaseRobotServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) { Kinova::Api::Base::BridgeList output; @@ -4191,7 +4191,7 @@ bool BaseServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, ko return true; } -bool BaseServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) +bool BaseRobotServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) { Kinova::Api::Base::BridgeIdentifier input; @@ -4226,7 +4226,7 @@ bool BaseServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req return true; } -bool BaseServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) +bool BaseRobotServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) { Kinova::Api::Base::PreComputedJointTrajectory input; @@ -4258,7 +4258,7 @@ bool BaseServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputed return true; } -bool BaseServices::GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) +bool BaseRobotServices::GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) { Kinova::Api::ProductConfiguration::CompleteProductConfiguration output; @@ -4291,7 +4291,7 @@ bool BaseServices::GetProductConfiguration(kortex_driver::GetProductConfiguratio return true; } -bool BaseServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) +bool BaseRobotServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) { Kinova::Api::ProductConfiguration::ProductConfigurationEndEffectorType input; @@ -4323,7 +4323,7 @@ bool BaseServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEf return true; } -bool BaseServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) +bool BaseRobotServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) { kortex_driver::KortexError result_error; @@ -4353,7 +4353,7 @@ bool BaseServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFact return true; } -bool BaseServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) +bool BaseRobotServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) { Kinova::Api::Base::TrajectoryErrorReport output; @@ -4386,9 +4386,9 @@ bool BaseServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorRep return true; } -bool BaseServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) +bool BaseRobotServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_speed_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_speed_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -4420,9 +4420,9 @@ bool BaseServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSp return true; } -bool BaseServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) +bool BaseRobotServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_torque_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_torque_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -4454,9 +4454,9 @@ bool BaseServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsT return true; } -bool BaseServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) +bool BaseRobotServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) { - ROS_WARN("The base/get_twist_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_twist_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::TwistLimitation output; @@ -4488,9 +4488,9 @@ bool BaseServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation: return true; } -bool BaseServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) +bool BaseRobotServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) { - ROS_WARN("The base/get_wrench_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_wrench_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::WrenchLimitation output; @@ -4522,7 +4522,7 @@ bool BaseServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitatio return true; } -bool BaseServices::SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) +bool BaseRobotServices::SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) { Kinova::Api::Base::ControllerConfigurationMode input; @@ -4554,7 +4554,7 @@ bool BaseServices::SetControllerConfigurationMode(kortex_driver::SetControllerCo return true; } -bool BaseServices::GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) +bool BaseRobotServices::GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) { Kinova::Api::Base::ControllerConfigurationMode output; @@ -4587,7 +4587,7 @@ bool BaseServices::GetControllerConfigurationMode(kortex_driver::GetControllerCo return true; } -bool BaseServices::StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) +bool BaseRobotServices::StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) { Kinova::Api::Base::SequenceTaskHandle input; @@ -4619,7 +4619,7 @@ bool BaseServices::StartTeaching(kortex_driver::StartTeaching::Request &req, ko return true; } -bool BaseServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) +bool BaseRobotServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) { kortex_driver::KortexError result_error; @@ -4649,7 +4649,7 @@ bool BaseServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kort return true; } -bool BaseServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) +bool BaseRobotServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) { Kinova::Api::Base::SequenceTasksConfiguration input; @@ -4684,7 +4684,7 @@ bool BaseServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &r return true; } -bool BaseServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) +bool BaseRobotServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) { Kinova::Api::Base::SequenceTaskConfiguration input; @@ -4716,7 +4716,7 @@ bool BaseServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request return true; } -bool BaseServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) +bool BaseRobotServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) { Kinova::Api::Base::SequenceTasksPair input; @@ -4748,7 +4748,7 @@ bool BaseServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request return true; } -bool BaseServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) +bool BaseRobotServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) { Kinova::Api::Base::SequenceTaskHandle input; @@ -4783,7 +4783,7 @@ bool BaseServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &r return true; } -bool BaseServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) +bool BaseRobotServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -4818,7 +4818,7 @@ bool BaseServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Req return true; } -bool BaseServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) +bool BaseRobotServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) { Kinova::Api::Base::SequenceTaskHandle input; @@ -4850,7 +4850,7 @@ bool BaseServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request return true; } -bool BaseServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) +bool BaseRobotServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -4882,7 +4882,7 @@ bool BaseServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks: return true; } -bool BaseServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) +bool BaseRobotServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) { Kinova::Api::Base::Snapshot input; @@ -4914,7 +4914,7 @@ bool BaseServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kort return true; } -bool BaseServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) +bool BaseRobotServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) { Kinova::Api::Base::FirmwareBundleVersions output; @@ -4947,7 +4947,7 @@ bool BaseServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVer return true; } -bool BaseServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) +bool BaseRobotServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) { Kinova::Api::Base::SequenceTasksPair input; @@ -4979,7 +4979,7 @@ bool BaseServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &r return true; } -bool BaseServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) +bool BaseRobotServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -5014,7 +5014,7 @@ bool BaseServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &r return true; } -bool BaseServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) +bool BaseRobotServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) { Kinova::Api::Base::MapHandle input; @@ -5049,7 +5049,7 @@ bool BaseServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kort return true; } -bool BaseServices::SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) +bool BaseRobotServices::SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) { Kinova::Api::Base::ControllerConfiguration input; @@ -5081,7 +5081,7 @@ bool BaseServices::SetControllerConfiguration(kortex_driver::SetControllerConfig return true; } -bool BaseServices::GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) +bool BaseRobotServices::GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) { Kinova::Api::Base::ControllerHandle input; @@ -5116,7 +5116,7 @@ bool BaseServices::GetControllerConfiguration(kortex_driver::GetControllerConfig return true; } -bool BaseServices::GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) +bool BaseRobotServices::GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) { Kinova::Api::Base::ControllerConfigurationList output; diff --git a/kortex_driver/src/generated/basecyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/basecyclic_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/basecyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/basecyclic_proto_converter.cpp index 321ab90f..30f9bc4e 100644 --- a/kortex_driver/src/generated/basecyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/basecyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" int ToProtoData(kortex_driver::ActuatorCommand input, Kinova::Api::BaseCyclic::ActuatorCommand *output) { diff --git a/kortex_driver/src/generated/basecyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/basecyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp index 6399d0b7..e064758e 100644 --- a/kortex_driver/src/generated/basecyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" int ToRosData(Kinova::Api::BaseCyclic::ActuatorCommand input, kortex_driver::ActuatorCommand &output) { diff --git a/kortex_driver/src/generated/common_proto_converter.cpp b/kortex_driver/src/generated/robot/common_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/common_proto_converter.cpp rename to kortex_driver/src/generated/robot/common_proto_converter.cpp index e0211113..d1357870 100644 --- a/kortex_driver/src/generated/common_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/common_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" int ToProtoData(kortex_driver::DeviceHandle input, Kinova::Api::Common::DeviceHandle *output) { diff --git a/kortex_driver/src/generated/common_ros_converter.cpp b/kortex_driver/src/generated/robot/common_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/common_ros_converter.cpp rename to kortex_driver/src/generated/robot/common_ros_converter.cpp index d9cb8e82..df98ecba 100644 --- a/kortex_driver/src/generated/common_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/common_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" int ToRosData(Kinova::Api::Common::DeviceHandle input, kortex_driver::DeviceHandle &output) { diff --git a/kortex_driver/src/generated/controlconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/controlconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp index c6541423..844f103d 100644 --- a/kortex_driver/src/generated/controlconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" int ToProtoData(kortex_driver::GravityVector input, Kinova::Api::ControlConfig::GravityVector *output) { diff --git a/kortex_driver/src/generated/controlconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/controlconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp index 531310d7..5032d465 100644 --- a/kortex_driver/src/generated/controlconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" int ToRosData(Kinova::Api::ControlConfig::GravityVector input, kortex_driver::GravityVector &output) { diff --git a/kortex_driver/src/generated/controlconfig_services.cpp b/kortex_driver/src/generated/robot/controlconfig_services.cpp similarity index 68% rename from kortex_driver/src/generated/controlconfig_services.cpp rename to kortex_driver/src/generated/robot/controlconfig_services.cpp index eb06f4f2..0e75bf8f 100644 --- a/kortex_driver/src/generated/controlconfig_services.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_services.cpp @@ -14,87 +14,87 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/controlconfig_services.h" - -ControlConfigServices::ControlConfigServices(ros::NodeHandle& n, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_services.h" + +ControlConfigRobotServices::ControlConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms): + IControlConfigServices(node_handle), m_controlconfig(controlconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_ControlConfigurationTopic = m_n.advertise("control_configuration_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ControlConfigurationTopic = m_node_handle.advertise("control_configuration_topic", 1000); m_is_activated_ControlConfigurationTopic = false; - m_serviceSetDeviceID = n.advertiseService("control_config/set_device_id", &ControlConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("control_config/set_api_options", &ControlConfigServices::SetApiOptions, this); - - m_serviceSetGravityVector = m_n.advertiseService("control_config/set_gravity_vector", &ControlConfigServices::SetGravityVector, this); - m_serviceGetGravityVector = m_n.advertiseService("control_config/get_gravity_vector", &ControlConfigServices::GetGravityVector, this); - m_serviceSetPayloadInformation = m_n.advertiseService("control_config/set_payload_information", &ControlConfigServices::SetPayloadInformation, this); - m_serviceGetPayloadInformation = m_n.advertiseService("control_config/get_payload_information", &ControlConfigServices::GetPayloadInformation, this); - m_serviceSetToolConfiguration = m_n.advertiseService("control_config/set_tool_configuration", &ControlConfigServices::SetToolConfiguration, this); - m_serviceGetToolConfiguration = m_n.advertiseService("control_config/get_tool_configuration", &ControlConfigServices::GetToolConfiguration, this); - m_serviceOnNotificationControlConfigurationTopic = m_n.advertiseService("control_config/activate_publishing_of_control_configuration_topic", &ControlConfigServices::OnNotificationControlConfigurationTopic, this); - m_serviceControlConfig_Unsubscribe = m_n.advertiseService("control_config/unsubscribe", &ControlConfigServices::ControlConfig_Unsubscribe, this); - m_serviceSetCartesianReferenceFrame = m_n.advertiseService("control_config/set_cartesian_reference_frame", &ControlConfigServices::SetCartesianReferenceFrame, this); - m_serviceGetCartesianReferenceFrame = m_n.advertiseService("control_config/get_cartesian_reference_frame", &ControlConfigServices::GetCartesianReferenceFrame, this); - m_serviceControlConfig_GetControlMode = m_n.advertiseService("control_config/get_control_mode", &ControlConfigServices::ControlConfig_GetControlMode, this); - m_serviceSetJointSpeedSoftLimits = m_n.advertiseService("control_config/set_joint_speed_soft_limits", &ControlConfigServices::SetJointSpeedSoftLimits, this); - m_serviceSetTwistLinearSoftLimit = m_n.advertiseService("control_config/set_twist_linear_soft_limit", &ControlConfigServices::SetTwistLinearSoftLimit, this); - m_serviceSetTwistAngularSoftLimit = m_n.advertiseService("control_config/set_twist_angular_soft_limit", &ControlConfigServices::SetTwistAngularSoftLimit, this); - m_serviceSetJointAccelerationSoftLimits = m_n.advertiseService("control_config/set_joint_acceleration_soft_limits", &ControlConfigServices::SetJointAccelerationSoftLimits, this); - m_serviceGetKinematicHardLimits = m_n.advertiseService("control_config/get_kinematic_hard_limits", &ControlConfigServices::GetKinematicHardLimits, this); - m_serviceGetKinematicSoftLimits = m_n.advertiseService("control_config/get_kinematic_soft_limits", &ControlConfigServices::GetKinematicSoftLimits, this); - m_serviceGetAllKinematicSoftLimits = m_n.advertiseService("control_config/get_all_kinematic_soft_limits", &ControlConfigServices::GetAllKinematicSoftLimits, this); - m_serviceSetDesiredLinearTwist = m_n.advertiseService("control_config/set_desired_linear_twist", &ControlConfigServices::SetDesiredLinearTwist, this); - m_serviceSetDesiredAngularTwist = m_n.advertiseService("control_config/set_desired_angular_twist", &ControlConfigServices::SetDesiredAngularTwist, this); - m_serviceSetDesiredJointSpeeds = m_n.advertiseService("control_config/set_desired_joint_speeds", &ControlConfigServices::SetDesiredJointSpeeds, this); - m_serviceGetDesiredSpeeds = m_n.advertiseService("control_config/get_desired_speeds", &ControlConfigServices::GetDesiredSpeeds, this); - m_serviceResetGravityVector = m_n.advertiseService("control_config/reset_gravity_vector", &ControlConfigServices::ResetGravityVector, this); - m_serviceResetPayloadInformation = m_n.advertiseService("control_config/reset_payload_information", &ControlConfigServices::ResetPayloadInformation, this); - m_serviceResetToolConfiguration = m_n.advertiseService("control_config/reset_tool_configuration", &ControlConfigServices::ResetToolConfiguration, this); - m_serviceResetJointSpeedSoftLimits = m_n.advertiseService("control_config/reset_joint_speed_soft_limits", &ControlConfigServices::ResetJointSpeedSoftLimits, this); - m_serviceResetTwistLinearSoftLimit = m_n.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigServices::ResetTwistLinearSoftLimit, this); - m_serviceResetTwistAngularSoftLimit = m_n.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigServices::ResetTwistAngularSoftLimit, this); - m_serviceResetJointAccelerationSoftLimits = m_n.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigServices::ResetJointAccelerationSoftLimits, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("control_config/set_device_id", &ControlConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("control_config/set_api_options", &ControlConfigRobotServices::SetApiOptions, this); + + m_serviceSetGravityVector = m_node_handle.advertiseService("control_config/set_gravity_vector", &ControlConfigRobotServices::SetGravityVector, this); + m_serviceGetGravityVector = m_node_handle.advertiseService("control_config/get_gravity_vector", &ControlConfigRobotServices::GetGravityVector, this); + m_serviceSetPayloadInformation = m_node_handle.advertiseService("control_config/set_payload_information", &ControlConfigRobotServices::SetPayloadInformation, this); + m_serviceGetPayloadInformation = m_node_handle.advertiseService("control_config/get_payload_information", &ControlConfigRobotServices::GetPayloadInformation, this); + m_serviceSetToolConfiguration = m_node_handle.advertiseService("control_config/set_tool_configuration", &ControlConfigRobotServices::SetToolConfiguration, this); + m_serviceGetToolConfiguration = m_node_handle.advertiseService("control_config/get_tool_configuration", &ControlConfigRobotServices::GetToolConfiguration, this); + m_serviceOnNotificationControlConfigurationTopic = m_node_handle.advertiseService("control_config/activate_publishing_of_control_configuration_topic", &ControlConfigRobotServices::OnNotificationControlConfigurationTopic, this); + m_serviceControlConfig_Unsubscribe = m_node_handle.advertiseService("control_config/unsubscribe", &ControlConfigRobotServices::ControlConfig_Unsubscribe, this); + m_serviceSetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/set_cartesian_reference_frame", &ControlConfigRobotServices::SetCartesianReferenceFrame, this); + m_serviceGetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/get_cartesian_reference_frame", &ControlConfigRobotServices::GetCartesianReferenceFrame, this); + m_serviceControlConfig_GetControlMode = m_node_handle.advertiseService("control_config/get_control_mode", &ControlConfigRobotServices::ControlConfig_GetControlMode, this); + m_serviceSetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/set_joint_speed_soft_limits", &ControlConfigRobotServices::SetJointSpeedSoftLimits, this); + m_serviceSetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/set_twist_linear_soft_limit", &ControlConfigRobotServices::SetTwistLinearSoftLimit, this); + m_serviceSetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/set_twist_angular_soft_limit", &ControlConfigRobotServices::SetTwistAngularSoftLimit, this); + m_serviceSetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/set_joint_acceleration_soft_limits", &ControlConfigRobotServices::SetJointAccelerationSoftLimits, this); + m_serviceGetKinematicHardLimits = m_node_handle.advertiseService("control_config/get_kinematic_hard_limits", &ControlConfigRobotServices::GetKinematicHardLimits, this); + m_serviceGetKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_kinematic_soft_limits", &ControlConfigRobotServices::GetKinematicSoftLimits, this); + m_serviceGetAllKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_all_kinematic_soft_limits", &ControlConfigRobotServices::GetAllKinematicSoftLimits, this); + m_serviceSetDesiredLinearTwist = m_node_handle.advertiseService("control_config/set_desired_linear_twist", &ControlConfigRobotServices::SetDesiredLinearTwist, this); + m_serviceSetDesiredAngularTwist = m_node_handle.advertiseService("control_config/set_desired_angular_twist", &ControlConfigRobotServices::SetDesiredAngularTwist, this); + m_serviceSetDesiredJointSpeeds = m_node_handle.advertiseService("control_config/set_desired_joint_speeds", &ControlConfigRobotServices::SetDesiredJointSpeeds, this); + m_serviceGetDesiredSpeeds = m_node_handle.advertiseService("control_config/get_desired_speeds", &ControlConfigRobotServices::GetDesiredSpeeds, this); + m_serviceResetGravityVector = m_node_handle.advertiseService("control_config/reset_gravity_vector", &ControlConfigRobotServices::ResetGravityVector, this); + m_serviceResetPayloadInformation = m_node_handle.advertiseService("control_config/reset_payload_information", &ControlConfigRobotServices::ResetPayloadInformation, this); + m_serviceResetToolConfiguration = m_node_handle.advertiseService("control_config/reset_tool_configuration", &ControlConfigRobotServices::ResetToolConfiguration, this); + m_serviceResetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_speed_soft_limits", &ControlConfigRobotServices::ResetJointSpeedSoftLimits, this); + m_serviceResetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigRobotServices::ResetTwistLinearSoftLimit, this); + m_serviceResetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigRobotServices::ResetTwistAngularSoftLimit, this); + m_serviceResetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigRobotServices::ResetJointAccelerationSoftLimits, this); } -bool ControlConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool ControlConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool ControlConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool ControlConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -102,7 +102,7 @@ bool ControlConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool ControlConfigServices::SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) +bool ControlConfigRobotServices::SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) { Kinova::Api::ControlConfig::GravityVector input; @@ -134,7 +134,7 @@ bool ControlConfigServices::SetGravityVector(kortex_driver::SetGravityVector::Re return true; } -bool ControlConfigServices::GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) +bool ControlConfigRobotServices::GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) { Kinova::Api::ControlConfig::GravityVector output; @@ -167,7 +167,7 @@ bool ControlConfigServices::GetGravityVector(kortex_driver::GetGravityVector::Re return true; } -bool ControlConfigServices::SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) +bool ControlConfigRobotServices::SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) { Kinova::Api::ControlConfig::PayloadInformation input; @@ -199,7 +199,7 @@ bool ControlConfigServices::SetPayloadInformation(kortex_driver::SetPayloadInfor return true; } -bool ControlConfigServices::GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) +bool ControlConfigRobotServices::GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) { Kinova::Api::ControlConfig::PayloadInformation output; @@ -232,7 +232,7 @@ bool ControlConfigServices::GetPayloadInformation(kortex_driver::GetPayloadInfor return true; } -bool ControlConfigServices::SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) +bool ControlConfigRobotServices::SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) { Kinova::Api::ControlConfig::ToolConfiguration input; @@ -264,7 +264,7 @@ bool ControlConfigServices::SetToolConfiguration(kortex_driver::SetToolConfigura return true; } -bool ControlConfigServices::GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) +bool ControlConfigRobotServices::GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) { Kinova::Api::ControlConfig::ToolConfiguration output; @@ -297,7 +297,7 @@ bool ControlConfigServices::GetToolConfiguration(kortex_driver::GetToolConfigura return true; } -bool ControlConfigServices::OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) +bool ControlConfigRobotServices::OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -311,7 +311,7 @@ bool ControlConfigServices::OnNotificationControlConfigurationTopic(kortex_drive try { - std::function< void (Kinova::Api::ControlConfig::ControlConfigurationNotification) > callback = std::bind(&ControlConfigServices::cb_ControlConfigurationTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::ControlConfig::ControlConfigurationNotification) > callback = std::bind(&ControlConfigRobotServices::cb_ControlConfigurationTopic, this, std::placeholders::_1); output = m_controlconfig->OnNotificationControlConfigurationTopic(callback, input, m_current_device_id); m_is_activated_ControlConfigurationTopic = true; } @@ -336,14 +336,14 @@ bool ControlConfigServices::OnNotificationControlConfigurationTopic(kortex_drive ToRosData(output, res.output); return true; } -void ControlConfigServices::cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) +void ControlConfigRobotServices::cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) { kortex_driver::ControlConfigurationNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControlConfigurationTopic.publish(ros_msg); } -bool ControlConfigServices::ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) +bool ControlConfigRobotServices::ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) { Kinova::Api::Common::NotificationHandle input; @@ -375,7 +375,7 @@ bool ControlConfigServices::ControlConfig_Unsubscribe(kortex_driver::ControlConf return true; } -bool ControlConfigServices::SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) +bool ControlConfigRobotServices::SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) { Kinova::Api::ControlConfig::CartesianReferenceFrameInfo input; @@ -407,7 +407,7 @@ bool ControlConfigServices::SetCartesianReferenceFrame(kortex_driver::SetCartesi return true; } -bool ControlConfigServices::GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) +bool ControlConfigRobotServices::GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) { Kinova::Api::ControlConfig::CartesianReferenceFrameInfo output; @@ -440,7 +440,7 @@ bool ControlConfigServices::GetCartesianReferenceFrame(kortex_driver::GetCartesi return true; } -bool ControlConfigServices::ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) +bool ControlConfigRobotServices::ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation output; @@ -473,7 +473,7 @@ bool ControlConfigServices::ControlConfig_GetControlMode(kortex_driver::ControlC return true; } -bool ControlConfigServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) +bool ControlConfigRobotServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) { Kinova::Api::ControlConfig::JointSpeedSoftLimits input; @@ -505,7 +505,7 @@ bool ControlConfigServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeed return true; } -bool ControlConfigServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) +bool ControlConfigRobotServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) { Kinova::Api::ControlConfig::TwistLinearSoftLimit input; @@ -537,7 +537,7 @@ bool ControlConfigServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinea return true; } -bool ControlConfigServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) +bool ControlConfigRobotServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) { Kinova::Api::ControlConfig::TwistAngularSoftLimit input; @@ -569,7 +569,7 @@ bool ControlConfigServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngu return true; } -bool ControlConfigServices::SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) +bool ControlConfigRobotServices::SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) { Kinova::Api::ControlConfig::JointAccelerationSoftLimits input; @@ -601,7 +601,7 @@ bool ControlConfigServices::SetJointAccelerationSoftLimits(kortex_driver::SetJoi return true; } -bool ControlConfigServices::GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) +bool ControlConfigRobotServices::GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) { Kinova::Api::ControlConfig::KinematicLimits output; @@ -634,7 +634,7 @@ bool ControlConfigServices::GetKinematicHardLimits(kortex_driver::GetKinematicHa return true; } -bool ControlConfigServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) +bool ControlConfigRobotServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -669,7 +669,7 @@ bool ControlConfigServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSo return true; } -bool ControlConfigServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) +bool ControlConfigRobotServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) { Kinova::Api::ControlConfig::KinematicLimitsList output; @@ -702,7 +702,7 @@ bool ControlConfigServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinem return true; } -bool ControlConfigServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) +bool ControlConfigRobotServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) { Kinova::Api::ControlConfig::LinearTwist input; @@ -734,7 +734,7 @@ bool ControlConfigServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinea return true; } -bool ControlConfigServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) +bool ControlConfigRobotServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) { Kinova::Api::ControlConfig::AngularTwist input; @@ -766,7 +766,7 @@ bool ControlConfigServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngu return true; } -bool ControlConfigServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) +bool ControlConfigRobotServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) { Kinova::Api::ControlConfig::JointSpeeds input; @@ -798,7 +798,7 @@ bool ControlConfigServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJoint return true; } -bool ControlConfigServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) +bool ControlConfigRobotServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) { Kinova::Api::ControlConfig::DesiredSpeeds output; @@ -831,7 +831,7 @@ bool ControlConfigServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Re return true; } -bool ControlConfigServices::ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) +bool ControlConfigRobotServices::ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) { Kinova::Api::ControlConfig::GravityVector output; @@ -864,7 +864,7 @@ bool ControlConfigServices::ResetGravityVector(kortex_driver::ResetGravityVector return true; } -bool ControlConfigServices::ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) +bool ControlConfigRobotServices::ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) { Kinova::Api::ControlConfig::PayloadInformation output; @@ -897,7 +897,7 @@ bool ControlConfigServices::ResetPayloadInformation(kortex_driver::ResetPayloadI return true; } -bool ControlConfigServices::ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) +bool ControlConfigRobotServices::ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) { Kinova::Api::ControlConfig::ToolConfiguration output; @@ -930,7 +930,7 @@ bool ControlConfigServices::ResetToolConfiguration(kortex_driver::ResetToolConfi return true; } -bool ControlConfigServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) +bool ControlConfigRobotServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -965,7 +965,7 @@ bool ControlConfigServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointS return true; } -bool ControlConfigServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) +bool ControlConfigRobotServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -1000,7 +1000,7 @@ bool ControlConfigServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistL return true; } -bool ControlConfigServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) +bool ControlConfigRobotServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -1035,7 +1035,7 @@ bool ControlConfigServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwist return true; } -bool ControlConfigServices::ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) +bool ControlConfigRobotServices::ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; diff --git a/kortex_driver/src/generated/deviceconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/deviceconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/deviceconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/deviceconfig_proto_converter.cpp index 636e6201..143b5902 100644 --- a/kortex_driver/src/generated/deviceconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" int ToProtoData(kortex_driver::DeviceType input, Kinova::Api::DeviceConfig::DeviceType *output) { diff --git a/kortex_driver/src/generated/deviceconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/deviceconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp index 49829cc8..15e0ef16 100644 --- a/kortex_driver/src/generated/deviceconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" int ToRosData(Kinova::Api::DeviceConfig::DeviceType input, kortex_driver::DeviceType &output) { diff --git a/kortex_driver/src/generated/deviceconfig_services.cpp b/kortex_driver/src/generated/robot/deviceconfig_services.cpp similarity index 70% rename from kortex_driver/src/generated/deviceconfig_services.cpp rename to kortex_driver/src/generated/robot/deviceconfig_services.cpp index 1380c329..8476cff6 100644 --- a/kortex_driver/src/generated/deviceconfig_services.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_services.cpp @@ -14,90 +14,90 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_services.h" - -DeviceConfigServices::DeviceConfigServices(ros::NodeHandle& n, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_services.h" + +DeviceConfigRobotServices::DeviceConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms): + IDeviceConfigServices(node_handle), m_deviceconfig(deviceconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_SafetyTopic = m_n.advertise("safety_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_SafetyTopic = m_node_handle.advertise("safety_topic", 1000); m_is_activated_SafetyTopic = false; - m_serviceSetDeviceID = n.advertiseService("device_config/set_device_id", &DeviceConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("device_config/set_api_options", &DeviceConfigServices::SetApiOptions, this); - - m_serviceGetRunMode = m_n.advertiseService("device_config/get_run_mode", &DeviceConfigServices::GetRunMode, this); - m_serviceSetRunMode = m_n.advertiseService("device_config/set_run_mode", &DeviceConfigServices::SetRunMode, this); - m_serviceGetDeviceType = m_n.advertiseService("device_config/get_device_type", &DeviceConfigServices::GetDeviceType, this); - m_serviceGetFirmwareVersion = m_n.advertiseService("device_config/get_firmware_version", &DeviceConfigServices::GetFirmwareVersion, this); - m_serviceGetBootloaderVersion = m_n.advertiseService("device_config/get_bootloader_version", &DeviceConfigServices::GetBootloaderVersion, this); - m_serviceGetModelNumber = m_n.advertiseService("device_config/get_model_number", &DeviceConfigServices::GetModelNumber, this); - m_serviceGetPartNumber = m_n.advertiseService("device_config/get_part_number", &DeviceConfigServices::GetPartNumber, this); - m_serviceGetSerialNumber = m_n.advertiseService("device_config/get_serial_number", &DeviceConfigServices::GetSerialNumber, this); - m_serviceGetMACAddress = m_n.advertiseService("device_config/get_m_a_c_address", &DeviceConfigServices::GetMACAddress, this); - m_serviceGetIPv4Settings = m_n.advertiseService("device_config/get_i_pv4_settings", &DeviceConfigServices::GetIPv4Settings, this); - m_serviceSetIPv4Settings = m_n.advertiseService("device_config/set_i_pv4_settings", &DeviceConfigServices::SetIPv4Settings, this); - m_serviceGetPartNumberRevision = m_n.advertiseService("device_config/get_part_number_revision", &DeviceConfigServices::GetPartNumberRevision, this); - m_serviceRebootRequest = m_n.advertiseService("device_config/reboot_request", &DeviceConfigServices::RebootRequest, this); - m_serviceSetSafetyEnable = m_n.advertiseService("device_config/set_safety_enable", &DeviceConfigServices::SetSafetyEnable, this); - m_serviceSetSafetyErrorThreshold = m_n.advertiseService("device_config/set_safety_error_threshold", &DeviceConfigServices::SetSafetyErrorThreshold, this); - m_serviceSetSafetyWarningThreshold = m_n.advertiseService("device_config/set_safety_warning_threshold", &DeviceConfigServices::SetSafetyWarningThreshold, this); - m_serviceSetSafetyConfiguration = m_n.advertiseService("device_config/set_safety_configuration", &DeviceConfigServices::SetSafetyConfiguration, this); - m_serviceGetSafetyConfiguration = m_n.advertiseService("device_config/get_safety_configuration", &DeviceConfigServices::GetSafetyConfiguration, this); - m_serviceGetSafetyInformation = m_n.advertiseService("device_config/get_safety_information", &DeviceConfigServices::GetSafetyInformation, this); - m_serviceGetSafetyEnable = m_n.advertiseService("device_config/get_safety_enable", &DeviceConfigServices::GetSafetyEnable, this); - m_serviceGetSafetyStatus = m_n.advertiseService("device_config/get_safety_status", &DeviceConfigServices::GetSafetyStatus, this); - m_serviceClearAllSafetyStatus = m_n.advertiseService("device_config/clear_all_safety_status", &DeviceConfigServices::ClearAllSafetyStatus, this); - m_serviceClearSafetyStatus = m_n.advertiseService("device_config/clear_safety_status", &DeviceConfigServices::ClearSafetyStatus, this); - m_serviceGetAllSafetyConfiguration = m_n.advertiseService("device_config/get_all_safety_configuration", &DeviceConfigServices::GetAllSafetyConfiguration, this); - m_serviceGetAllSafetyInformation = m_n.advertiseService("device_config/get_all_safety_information", &DeviceConfigServices::GetAllSafetyInformation, this); - m_serviceResetSafetyDefaults = m_n.advertiseService("device_config/reset_safety_defaults", &DeviceConfigServices::ResetSafetyDefaults, this); - m_serviceOnNotificationSafetyTopic = m_n.advertiseService("device_config/activate_publishing_of_safety_topic", &DeviceConfigServices::OnNotificationSafetyTopic, this); - m_serviceExecuteCalibration = m_n.advertiseService("device_config/execute_calibration", &DeviceConfigServices::ExecuteCalibration, this); - m_serviceGetCalibrationResult = m_n.advertiseService("device_config/get_calibration_result", &DeviceConfigServices::GetCalibrationResult, this); - m_serviceStopCalibration = m_n.advertiseService("device_config/stop_calibration", &DeviceConfigServices::StopCalibration, this); - m_serviceDeviceConfig_SetCapSenseConfig = m_n.advertiseService("device_config/set_cap_sense_config", &DeviceConfigServices::DeviceConfig_SetCapSenseConfig, this); - m_serviceDeviceConfig_GetCapSenseConfig = m_n.advertiseService("device_config/get_cap_sense_config", &DeviceConfigServices::DeviceConfig_GetCapSenseConfig, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("device_config/set_device_id", &DeviceConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_config/set_api_options", &DeviceConfigRobotServices::SetApiOptions, this); + + m_serviceGetRunMode = m_node_handle.advertiseService("device_config/get_run_mode", &DeviceConfigRobotServices::GetRunMode, this); + m_serviceSetRunMode = m_node_handle.advertiseService("device_config/set_run_mode", &DeviceConfigRobotServices::SetRunMode, this); + m_serviceGetDeviceType = m_node_handle.advertiseService("device_config/get_device_type", &DeviceConfigRobotServices::GetDeviceType, this); + m_serviceGetFirmwareVersion = m_node_handle.advertiseService("device_config/get_firmware_version", &DeviceConfigRobotServices::GetFirmwareVersion, this); + m_serviceGetBootloaderVersion = m_node_handle.advertiseService("device_config/get_bootloader_version", &DeviceConfigRobotServices::GetBootloaderVersion, this); + m_serviceGetModelNumber = m_node_handle.advertiseService("device_config/get_model_number", &DeviceConfigRobotServices::GetModelNumber, this); + m_serviceGetPartNumber = m_node_handle.advertiseService("device_config/get_part_number", &DeviceConfigRobotServices::GetPartNumber, this); + m_serviceGetSerialNumber = m_node_handle.advertiseService("device_config/get_serial_number", &DeviceConfigRobotServices::GetSerialNumber, this); + m_serviceGetMACAddress = m_node_handle.advertiseService("device_config/get_m_a_c_address", &DeviceConfigRobotServices::GetMACAddress, this); + m_serviceGetIPv4Settings = m_node_handle.advertiseService("device_config/get_i_pv4_settings", &DeviceConfigRobotServices::GetIPv4Settings, this); + m_serviceSetIPv4Settings = m_node_handle.advertiseService("device_config/set_i_pv4_settings", &DeviceConfigRobotServices::SetIPv4Settings, this); + m_serviceGetPartNumberRevision = m_node_handle.advertiseService("device_config/get_part_number_revision", &DeviceConfigRobotServices::GetPartNumberRevision, this); + m_serviceRebootRequest = m_node_handle.advertiseService("device_config/reboot_request", &DeviceConfigRobotServices::RebootRequest, this); + m_serviceSetSafetyEnable = m_node_handle.advertiseService("device_config/set_safety_enable", &DeviceConfigRobotServices::SetSafetyEnable, this); + m_serviceSetSafetyErrorThreshold = m_node_handle.advertiseService("device_config/set_safety_error_threshold", &DeviceConfigRobotServices::SetSafetyErrorThreshold, this); + m_serviceSetSafetyWarningThreshold = m_node_handle.advertiseService("device_config/set_safety_warning_threshold", &DeviceConfigRobotServices::SetSafetyWarningThreshold, this); + m_serviceSetSafetyConfiguration = m_node_handle.advertiseService("device_config/set_safety_configuration", &DeviceConfigRobotServices::SetSafetyConfiguration, this); + m_serviceGetSafetyConfiguration = m_node_handle.advertiseService("device_config/get_safety_configuration", &DeviceConfigRobotServices::GetSafetyConfiguration, this); + m_serviceGetSafetyInformation = m_node_handle.advertiseService("device_config/get_safety_information", &DeviceConfigRobotServices::GetSafetyInformation, this); + m_serviceGetSafetyEnable = m_node_handle.advertiseService("device_config/get_safety_enable", &DeviceConfigRobotServices::GetSafetyEnable, this); + m_serviceGetSafetyStatus = m_node_handle.advertiseService("device_config/get_safety_status", &DeviceConfigRobotServices::GetSafetyStatus, this); + m_serviceClearAllSafetyStatus = m_node_handle.advertiseService("device_config/clear_all_safety_status", &DeviceConfigRobotServices::ClearAllSafetyStatus, this); + m_serviceClearSafetyStatus = m_node_handle.advertiseService("device_config/clear_safety_status", &DeviceConfigRobotServices::ClearSafetyStatus, this); + m_serviceGetAllSafetyConfiguration = m_node_handle.advertiseService("device_config/get_all_safety_configuration", &DeviceConfigRobotServices::GetAllSafetyConfiguration, this); + m_serviceGetAllSafetyInformation = m_node_handle.advertiseService("device_config/get_all_safety_information", &DeviceConfigRobotServices::GetAllSafetyInformation, this); + m_serviceResetSafetyDefaults = m_node_handle.advertiseService("device_config/reset_safety_defaults", &DeviceConfigRobotServices::ResetSafetyDefaults, this); + m_serviceOnNotificationSafetyTopic = m_node_handle.advertiseService("device_config/activate_publishing_of_safety_topic", &DeviceConfigRobotServices::OnNotificationSafetyTopic, this); + m_serviceExecuteCalibration = m_node_handle.advertiseService("device_config/execute_calibration", &DeviceConfigRobotServices::ExecuteCalibration, this); + m_serviceGetCalibrationResult = m_node_handle.advertiseService("device_config/get_calibration_result", &DeviceConfigRobotServices::GetCalibrationResult, this); + m_serviceStopCalibration = m_node_handle.advertiseService("device_config/stop_calibration", &DeviceConfigRobotServices::StopCalibration, this); + m_serviceDeviceConfig_SetCapSenseConfig = m_node_handle.advertiseService("device_config/set_cap_sense_config", &DeviceConfigRobotServices::DeviceConfig_SetCapSenseConfig, this); + m_serviceDeviceConfig_GetCapSenseConfig = m_node_handle.advertiseService("device_config/get_cap_sense_config", &DeviceConfigRobotServices::DeviceConfig_GetCapSenseConfig, this); } -bool DeviceConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool DeviceConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool DeviceConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool DeviceConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -105,7 +105,7 @@ bool DeviceConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool DeviceConfigServices::GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) +bool DeviceConfigRobotServices::GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) { Kinova::Api::DeviceConfig::RunMode output; @@ -138,7 +138,7 @@ bool DeviceConfigServices::GetRunMode(kortex_driver::GetRunMode::Request &req, return true; } -bool DeviceConfigServices::SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) +bool DeviceConfigRobotServices::SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) { Kinova::Api::DeviceConfig::RunMode input; @@ -170,7 +170,7 @@ bool DeviceConfigServices::SetRunMode(kortex_driver::SetRunMode::Request &req, return true; } -bool DeviceConfigServices::GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) +bool DeviceConfigRobotServices::GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) { Kinova::Api::DeviceConfig::DeviceType output; @@ -203,7 +203,7 @@ bool DeviceConfigServices::GetDeviceType(kortex_driver::GetDeviceType::Request return true; } -bool DeviceConfigServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) +bool DeviceConfigRobotServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) { Kinova::Api::DeviceConfig::FirmwareVersion output; @@ -236,7 +236,7 @@ bool DeviceConfigServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion: return true; } -bool DeviceConfigServices::GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) +bool DeviceConfigRobotServices::GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) { Kinova::Api::DeviceConfig::BootloaderVersion output; @@ -269,7 +269,7 @@ bool DeviceConfigServices::GetBootloaderVersion(kortex_driver::GetBootloaderVers return true; } -bool DeviceConfigServices::GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) +bool DeviceConfigRobotServices::GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) { Kinova::Api::DeviceConfig::ModelNumber output; @@ -302,7 +302,7 @@ bool DeviceConfigServices::GetModelNumber(kortex_driver::GetModelNumber::Request return true; } -bool DeviceConfigServices::GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) +bool DeviceConfigRobotServices::GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) { Kinova::Api::DeviceConfig::PartNumber output; @@ -335,7 +335,7 @@ bool DeviceConfigServices::GetPartNumber(kortex_driver::GetPartNumber::Request return true; } -bool DeviceConfigServices::GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) +bool DeviceConfigRobotServices::GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) { Kinova::Api::DeviceConfig::SerialNumber output; @@ -368,7 +368,7 @@ bool DeviceConfigServices::GetSerialNumber(kortex_driver::GetSerialNumber::Reque return true; } -bool DeviceConfigServices::GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) +bool DeviceConfigRobotServices::GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) { Kinova::Api::DeviceConfig::MACAddress output; @@ -401,7 +401,7 @@ bool DeviceConfigServices::GetMACAddress(kortex_driver::GetMACAddress::Request return true; } -bool DeviceConfigServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) +bool DeviceConfigRobotServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) { Kinova::Api::DeviceConfig::IPv4Settings output; @@ -434,7 +434,7 @@ bool DeviceConfigServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Reque return true; } -bool DeviceConfigServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) +bool DeviceConfigRobotServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) { Kinova::Api::DeviceConfig::IPv4Settings input; @@ -466,7 +466,7 @@ bool DeviceConfigServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Reque return true; } -bool DeviceConfigServices::GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) +bool DeviceConfigRobotServices::GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) { Kinova::Api::DeviceConfig::PartNumberRevision output; @@ -499,7 +499,7 @@ bool DeviceConfigServices::GetPartNumberRevision(kortex_driver::GetPartNumberRev return true; } -bool DeviceConfigServices::RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) +bool DeviceConfigRobotServices::RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) { Kinova::Api::DeviceConfig::RebootRqst input; @@ -531,7 +531,7 @@ bool DeviceConfigServices::RebootRequest(kortex_driver::RebootRequest::Request return true; } -bool DeviceConfigServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) +bool DeviceConfigRobotServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) { Kinova::Api::DeviceConfig::SafetyEnable input; @@ -563,7 +563,7 @@ bool DeviceConfigServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Reque return true; } -bool DeviceConfigServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) +bool DeviceConfigRobotServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) { Kinova::Api::DeviceConfig::SafetyThreshold input; @@ -595,7 +595,7 @@ bool DeviceConfigServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyError return true; } -bool DeviceConfigServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) +bool DeviceConfigRobotServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) { Kinova::Api::DeviceConfig::SafetyThreshold input; @@ -627,7 +627,7 @@ bool DeviceConfigServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWar return true; } -bool DeviceConfigServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) +bool DeviceConfigRobotServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) { Kinova::Api::DeviceConfig::SafetyConfiguration input; @@ -659,7 +659,7 @@ bool DeviceConfigServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfig return true; } -bool DeviceConfigServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) +bool DeviceConfigRobotServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -694,7 +694,7 @@ bool DeviceConfigServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfig return true; } -bool DeviceConfigServices::GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) +bool DeviceConfigRobotServices::GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -729,7 +729,7 @@ bool DeviceConfigServices::GetSafetyInformation(kortex_driver::GetSafetyInformat return true; } -bool DeviceConfigServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) +bool DeviceConfigRobotServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -764,7 +764,7 @@ bool DeviceConfigServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Reque return true; } -bool DeviceConfigServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) +bool DeviceConfigRobotServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -799,7 +799,7 @@ bool DeviceConfigServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Reque return true; } -bool DeviceConfigServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) +bool DeviceConfigRobotServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) { kortex_driver::KortexError result_error; @@ -829,7 +829,7 @@ bool DeviceConfigServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetySta return true; } -bool DeviceConfigServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) +bool DeviceConfigRobotServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -861,7 +861,7 @@ bool DeviceConfigServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::R return true; } -bool DeviceConfigServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) +bool DeviceConfigRobotServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) { Kinova::Api::DeviceConfig::SafetyConfigurationList output; @@ -894,7 +894,7 @@ bool DeviceConfigServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafety return true; } -bool DeviceConfigServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) +bool DeviceConfigRobotServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) { Kinova::Api::DeviceConfig::SafetyInformationList output; @@ -927,7 +927,7 @@ bool DeviceConfigServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyIn return true; } -bool DeviceConfigServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) +bool DeviceConfigRobotServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) { kortex_driver::KortexError result_error; @@ -957,7 +957,7 @@ bool DeviceConfigServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefault return true; } -bool DeviceConfigServices::OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) +bool DeviceConfigRobotServices::OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -971,7 +971,7 @@ bool DeviceConfigServices::OnNotificationSafetyTopic(kortex_driver::OnNotificati try { - std::function< void (Kinova::Api::Common::SafetyNotification) > callback = std::bind(&DeviceConfigServices::cb_SafetyTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Common::SafetyNotification) > callback = std::bind(&DeviceConfigRobotServices::cb_SafetyTopic, this, std::placeholders::_1); output = m_deviceconfig->OnNotificationSafetyTopic(callback, input, m_current_device_id); m_is_activated_SafetyTopic = true; } @@ -996,14 +996,14 @@ bool DeviceConfigServices::OnNotificationSafetyTopic(kortex_driver::OnNotificati ToRosData(output, res.output); return true; } -void DeviceConfigServices::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) +void DeviceConfigRobotServices::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) { kortex_driver::SafetyNotification ros_msg; ToRosData(notif, ros_msg); m_pub_SafetyTopic.publish(ros_msg); } -bool DeviceConfigServices::ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) +bool DeviceConfigRobotServices::ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) { Kinova::Api::DeviceConfig::Calibration input; @@ -1035,7 +1035,7 @@ bool DeviceConfigServices::ExecuteCalibration(kortex_driver::ExecuteCalibration: return true; } -bool DeviceConfigServices::GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) +bool DeviceConfigRobotServices::GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) { Kinova::Api::DeviceConfig::CalibrationElement input; @@ -1070,7 +1070,7 @@ bool DeviceConfigServices::GetCalibrationResult(kortex_driver::GetCalibrationRes return true; } -bool DeviceConfigServices::StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) +bool DeviceConfigRobotServices::StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) { Kinova::Api::DeviceConfig::Calibration input; @@ -1105,7 +1105,7 @@ bool DeviceConfigServices::StopCalibration(kortex_driver::StopCalibration::Reque return true; } -bool DeviceConfigServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) +bool DeviceConfigRobotServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) { Kinova::Api::DeviceConfig::CapSenseConfig input; @@ -1137,7 +1137,7 @@ bool DeviceConfigServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceC return true; } -bool DeviceConfigServices::DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) +bool DeviceConfigRobotServices::DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) { Kinova::Api::DeviceConfig::CapSenseConfig output; diff --git a/kortex_driver/src/generated/devicemanager_proto_converter.cpp b/kortex_driver/src/generated/robot/devicemanager_proto_converter.cpp similarity index 88% rename from kortex_driver/src/generated/devicemanager_proto_converter.cpp rename to kortex_driver/src/generated/robot/devicemanager_proto_converter.cpp index 31785e68..22cda421 100644 --- a/kortex_driver/src/generated/devicemanager_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/devicemanager_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" int ToProtoData(kortex_driver::DeviceHandles input, Kinova::Api::DeviceManager::DeviceHandles *output) { diff --git a/kortex_driver/src/generated/devicemanager_ros_converter.cpp b/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp similarity index 89% rename from kortex_driver/src/generated/devicemanager_ros_converter.cpp rename to kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp index c9a2115d..3b5e95f6 100644 --- a/kortex_driver/src/generated/devicemanager_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::DeviceHandles &output) { diff --git a/kortex_driver/src/generated/robot/devicemanager_services.cpp b/kortex_driver/src/generated/robot/devicemanager_services.cpp new file mode 100644 index 00000000..b649cf2b --- /dev/null +++ b/kortex_driver/src/generated/robot/devicemanager_services.cpp @@ -0,0 +1,106 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_services.h" + +DeviceManagerRobotServices::DeviceManagerRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms): + IDeviceManagerServices(node_handle), + m_devicemanager(devicemanager), + m_current_device_id(device_id) +{ + m_api_options.timeout_ms = timeout_ms; + + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("device_manager/set_device_id", &DeviceManagerRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_manager/set_api_options", &DeviceManagerRobotServices::SetApiOptions, this); + + m_serviceReadAllDevices = m_node_handle.advertiseService("device_manager/read_all_devices", &DeviceManagerRobotServices::ReadAllDevices, this); +} + +bool DeviceManagerRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + m_current_device_id = req.device_id; + + return true; +} + +bool DeviceManagerRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + m_api_options.timeout_ms = req.input.timeout_ms; + + return true; +} + + +bool DeviceManagerRobotServices::ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) +{ + + Kinova::Api::DeviceManager::DeviceHandles output; + + kortex_driver::KortexError result_error; + + try + { + output = m_devicemanager->ReadAllDevices(m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_driver/src/generated/grippercyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/grippercyclic_proto_converter.cpp similarity index 97% rename from kortex_driver/src/generated/grippercyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/grippercyclic_proto_converter.cpp index f5840258..fe1962d9 100644 --- a/kortex_driver/src/generated/grippercyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/grippercyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" int ToProtoData(kortex_driver::GripperCyclic_MessageId input, Kinova::Api::GripperCyclic::MessageId *output) { diff --git a/kortex_driver/src/generated/grippercyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/grippercyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp index 5dd097e2..09523044 100644 --- a/kortex_driver/src/generated/grippercyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" int ToRosData(Kinova::Api::GripperCyclic::MessageId input, kortex_driver::GripperCyclic_MessageId &output) { diff --git a/kortex_driver/src/generated/interconnectconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/interconnectconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp index e210b6a0..1c0bc732 100644 --- a/kortex_driver/src/generated/interconnectconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" int ToProtoData(kortex_driver::EthernetDeviceIdentification input, Kinova::Api::InterconnectConfig::EthernetDeviceIdentification *output) { diff --git a/kortex_driver/src/generated/interconnectconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/interconnectconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp index 356d96e4..bc2f660b 100644 --- a/kortex_driver/src/generated/interconnectconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" int ToRosData(Kinova::Api::InterconnectConfig::EthernetDeviceIdentification input, kortex_driver::EthernetDeviceIdentification &output) { diff --git a/kortex_driver/src/generated/interconnectconfig_services.cpp b/kortex_driver/src/generated/robot/interconnectconfig_services.cpp similarity index 65% rename from kortex_driver/src/generated/interconnectconfig_services.cpp rename to kortex_driver/src/generated/robot/interconnectconfig_services.cpp index c55847c2..bd696c48 100644 --- a/kortex_driver/src/generated/interconnectconfig_services.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_services.cpp @@ -14,70 +14,70 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_services.h" - -InterconnectConfigServices::InterconnectConfigServices(ros::NodeHandle& n, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_services.h" + +InterconnectConfigRobotServices::InterconnectConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms): + IInterconnectConfigServices(node_handle), m_interconnectconfig(interconnectconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - - m_serviceSetDeviceID = n.advertiseService("interconnect_config/set_device_id", &InterconnectConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("interconnect_config/set_api_options", &InterconnectConfigServices::SetApiOptions, this); - - m_serviceGetUARTConfiguration = m_n.advertiseService("interconnect_config/get_u_a_r_t_configuration", &InterconnectConfigServices::GetUARTConfiguration, this); - m_serviceSetUARTConfiguration = m_n.advertiseService("interconnect_config/set_u_a_r_t_configuration", &InterconnectConfigServices::SetUARTConfiguration, this); - m_serviceGetEthernetConfiguration = m_n.advertiseService("interconnect_config/get_ethernet_configuration", &InterconnectConfigServices::GetEthernetConfiguration, this); - m_serviceSetEthernetConfiguration = m_n.advertiseService("interconnect_config/set_ethernet_configuration", &InterconnectConfigServices::SetEthernetConfiguration, this); - m_serviceGetGPIOConfiguration = m_n.advertiseService("interconnect_config/get_g_p_i_o_configuration", &InterconnectConfigServices::GetGPIOConfiguration, this); - m_serviceSetGPIOConfiguration = m_n.advertiseService("interconnect_config/set_g_p_i_o_configuration", &InterconnectConfigServices::SetGPIOConfiguration, this); - m_serviceGetGPIOState = m_n.advertiseService("interconnect_config/get_g_p_i_o_state", &InterconnectConfigServices::GetGPIOState, this); - m_serviceSetGPIOState = m_n.advertiseService("interconnect_config/set_g_p_i_o_state", &InterconnectConfigServices::SetGPIOState, this); - m_serviceGetI2CConfiguration = m_n.advertiseService("interconnect_config/get_i2_c_configuration", &InterconnectConfigServices::GetI2CConfiguration, this); - m_serviceSetI2CConfiguration = m_n.advertiseService("interconnect_config/set_i2_c_configuration", &InterconnectConfigServices::SetI2CConfiguration, this); - m_serviceI2CRead = m_n.advertiseService("interconnect_config/i2_c_read", &InterconnectConfigServices::I2CRead, this); - m_serviceI2CReadRegister = m_n.advertiseService("interconnect_config/i2_c_read_register", &InterconnectConfigServices::I2CReadRegister, this); - m_serviceI2CWrite = m_n.advertiseService("interconnect_config/i2_c_write", &InterconnectConfigServices::I2CWrite, this); - m_serviceI2CWriteRegister = m_n.advertiseService("interconnect_config/i2_c_write_register", &InterconnectConfigServices::I2CWriteRegister, this); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("interconnect_config/set_device_id", &InterconnectConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("interconnect_config/set_api_options", &InterconnectConfigRobotServices::SetApiOptions, this); + + m_serviceGetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/get_u_a_r_t_configuration", &InterconnectConfigRobotServices::GetUARTConfiguration, this); + m_serviceSetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/set_u_a_r_t_configuration", &InterconnectConfigRobotServices::SetUARTConfiguration, this); + m_serviceGetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/get_ethernet_configuration", &InterconnectConfigRobotServices::GetEthernetConfiguration, this); + m_serviceSetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/set_ethernet_configuration", &InterconnectConfigRobotServices::SetEthernetConfiguration, this); + m_serviceGetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_configuration", &InterconnectConfigRobotServices::GetGPIOConfiguration, this); + m_serviceSetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_configuration", &InterconnectConfigRobotServices::SetGPIOConfiguration, this); + m_serviceGetGPIOState = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_state", &InterconnectConfigRobotServices::GetGPIOState, this); + m_serviceSetGPIOState = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_state", &InterconnectConfigRobotServices::SetGPIOState, this); + m_serviceGetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/get_i2_c_configuration", &InterconnectConfigRobotServices::GetI2CConfiguration, this); + m_serviceSetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/set_i2_c_configuration", &InterconnectConfigRobotServices::SetI2CConfiguration, this); + m_serviceI2CRead = m_node_handle.advertiseService("interconnect_config/i2_c_read", &InterconnectConfigRobotServices::I2CRead, this); + m_serviceI2CReadRegister = m_node_handle.advertiseService("interconnect_config/i2_c_read_register", &InterconnectConfigRobotServices::I2CReadRegister, this); + m_serviceI2CWrite = m_node_handle.advertiseService("interconnect_config/i2_c_write", &InterconnectConfigRobotServices::I2CWrite, this); + m_serviceI2CWriteRegister = m_node_handle.advertiseService("interconnect_config/i2_c_write_register", &InterconnectConfigRobotServices::I2CWriteRegister, this); } -bool InterconnectConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool InterconnectConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool InterconnectConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool InterconnectConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -85,7 +85,7 @@ bool InterconnectConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Req } -bool InterconnectConfigServices::GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) { Kinova::Api::Common::UARTDeviceIdentification input; @@ -120,7 +120,7 @@ bool InterconnectConfigServices::GetUARTConfiguration(kortex_driver::GetUARTConf return true; } -bool InterconnectConfigServices::SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) { Kinova::Api::Common::UARTConfiguration input; @@ -152,7 +152,7 @@ bool InterconnectConfigServices::SetUARTConfiguration(kortex_driver::SetUARTConf return true; } -bool InterconnectConfigServices::GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) { Kinova::Api::InterconnectConfig::EthernetDeviceIdentification input; @@ -187,7 +187,7 @@ bool InterconnectConfigServices::GetEthernetConfiguration(kortex_driver::GetEthe return true; } -bool InterconnectConfigServices::SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) { Kinova::Api::InterconnectConfig::EthernetConfiguration input; @@ -219,7 +219,7 @@ bool InterconnectConfigServices::SetEthernetConfiguration(kortex_driver::SetEthe return true; } -bool InterconnectConfigServices::GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) { Kinova::Api::InterconnectConfig::GPIOIdentification input; @@ -254,7 +254,7 @@ bool InterconnectConfigServices::GetGPIOConfiguration(kortex_driver::GetGPIOConf return true; } -bool InterconnectConfigServices::SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) { Kinova::Api::InterconnectConfig::GPIOConfiguration input; @@ -286,7 +286,7 @@ bool InterconnectConfigServices::SetGPIOConfiguration(kortex_driver::SetGPIOConf return true; } -bool InterconnectConfigServices::GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) +bool InterconnectConfigRobotServices::GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) { Kinova::Api::InterconnectConfig::GPIOIdentification input; @@ -321,7 +321,7 @@ bool InterconnectConfigServices::GetGPIOState(kortex_driver::GetGPIOState::Reque return true; } -bool InterconnectConfigServices::SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) +bool InterconnectConfigRobotServices::SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) { Kinova::Api::InterconnectConfig::GPIOState input; @@ -353,7 +353,7 @@ bool InterconnectConfigServices::SetGPIOState(kortex_driver::SetGPIOState::Reque return true; } -bool InterconnectConfigServices::GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) { Kinova::Api::InterconnectConfig::I2CDeviceIdentification input; @@ -388,7 +388,7 @@ bool InterconnectConfigServices::GetI2CConfiguration(kortex_driver::GetI2CConfig return true; } -bool InterconnectConfigServices::SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) { Kinova::Api::InterconnectConfig::I2CConfiguration input; @@ -420,7 +420,7 @@ bool InterconnectConfigServices::SetI2CConfiguration(kortex_driver::SetI2CConfig return true; } -bool InterconnectConfigServices::I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) +bool InterconnectConfigRobotServices::I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) { Kinova::Api::InterconnectConfig::I2CReadParameter input; @@ -455,7 +455,7 @@ bool InterconnectConfigServices::I2CRead(kortex_driver::I2CRead::Request &req, return true; } -bool InterconnectConfigServices::I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) +bool InterconnectConfigRobotServices::I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) { Kinova::Api::InterconnectConfig::I2CReadRegisterParameter input; @@ -490,7 +490,7 @@ bool InterconnectConfigServices::I2CReadRegister(kortex_driver::I2CReadRegister: return true; } -bool InterconnectConfigServices::I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) +bool InterconnectConfigRobotServices::I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) { Kinova::Api::InterconnectConfig::I2CWriteParameter input; @@ -522,7 +522,7 @@ bool InterconnectConfigServices::I2CWrite(kortex_driver::I2CWrite::Request &req return true; } -bool InterconnectConfigServices::I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) +bool InterconnectConfigRobotServices::I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) { Kinova::Api::InterconnectConfig::I2CWriteRegisterParameter input; diff --git a/kortex_driver/src/generated/interconnectcyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/interconnectcyclic_proto_converter.cpp similarity index 97% rename from kortex_driver/src/generated/interconnectcyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/interconnectcyclic_proto_converter.cpp index 2698cc16..a2aef974 100644 --- a/kortex_driver/src/generated/interconnectcyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectcyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" int ToProtoData(kortex_driver::InterconnectCyclic_MessageId input, Kinova::Api::InterconnectCyclic::MessageId *output) { diff --git a/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/interconnectcyclic_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/interconnectcyclic_ros_converter.cpp index 46688c53..bdda1db3 100644 --- a/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectcyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" int ToRosData(Kinova::Api::InterconnectCyclic::MessageId input, kortex_driver::InterconnectCyclic_MessageId &output) { diff --git a/kortex_driver/src/generated/productconfiguration_proto_converter.cpp b/kortex_driver/src/generated/robot/productconfiguration_proto_converter.cpp similarity index 95% rename from kortex_driver/src/generated/productconfiguration_proto_converter.cpp rename to kortex_driver/src/generated/robot/productconfiguration_proto_converter.cpp index 7fd28374..c93d9b8c 100644 --- a/kortex_driver/src/generated/productconfiguration_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/productconfiguration_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" int ToProtoData(kortex_driver::CompleteProductConfiguration input, Kinova::Api::ProductConfiguration::CompleteProductConfiguration *output) { diff --git a/kortex_driver/src/generated/productconfiguration_ros_converter.cpp b/kortex_driver/src/generated/robot/productconfiguration_ros_converter.cpp similarity index 94% rename from kortex_driver/src/generated/productconfiguration_ros_converter.cpp rename to kortex_driver/src/generated/robot/productconfiguration_ros_converter.cpp index 106fc819..4eeb2ad4 100644 --- a/kortex_driver/src/generated/productconfiguration_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/productconfiguration_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" int ToRosData(Kinova::Api::ProductConfiguration::CompleteProductConfiguration input, kortex_driver::CompleteProductConfiguration &output) { diff --git a/kortex_driver/src/generated/visionconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/visionconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/visionconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/visionconfig_proto_converter.cpp index 233a91c4..39009bbf 100644 --- a/kortex_driver/src/generated/visionconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/visionconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" int ToProtoData(kortex_driver::SensorSettings input, Kinova::Api::VisionConfig::SensorSettings *output) { diff --git a/kortex_driver/src/generated/visionconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/visionconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/visionconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/visionconfig_ros_converter.cpp index ff83444c..caee31c4 100644 --- a/kortex_driver/src/generated/visionconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/visionconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" int ToRosData(Kinova::Api::VisionConfig::SensorSettings input, kortex_driver::SensorSettings &output) { diff --git a/kortex_driver/src/generated/visionconfig_services.cpp b/kortex_driver/src/generated/robot/visionconfig_services.cpp similarity index 64% rename from kortex_driver/src/generated/visionconfig_services.cpp rename to kortex_driver/src/generated/robot/visionconfig_services.cpp index d5cba3a7..a128bfb7 100644 --- a/kortex_driver/src/generated/visionconfig_services.cpp +++ b/kortex_driver/src/generated/robot/visionconfig_services.cpp @@ -14,70 +14,70 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_services.h" - -VisionConfigServices::VisionConfigServices(ros::NodeHandle& n, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_services.h" + +VisionConfigRobotServices::VisionConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms): + IVisionConfigServices(node_handle), m_visionconfig(visionconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_VisionTopic = m_n.advertise("vision_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_VisionTopic = m_node_handle.advertise("vision_topic", 1000); m_is_activated_VisionTopic = false; - m_serviceSetDeviceID = n.advertiseService("vision_config/set_device_id", &VisionConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("vision_config/set_api_options", &VisionConfigServices::SetApiOptions, this); - - m_serviceSetSensorSettings = m_n.advertiseService("vision_config/set_sensor_settings", &VisionConfigServices::SetSensorSettings, this); - m_serviceGetSensorSettings = m_n.advertiseService("vision_config/get_sensor_settings", &VisionConfigServices::GetSensorSettings, this); - m_serviceGetOptionValue = m_n.advertiseService("vision_config/get_option_value", &VisionConfigServices::GetOptionValue, this); - m_serviceSetOptionValue = m_n.advertiseService("vision_config/set_option_value", &VisionConfigServices::SetOptionValue, this); - m_serviceGetOptionInformation = m_n.advertiseService("vision_config/get_option_information", &VisionConfigServices::GetOptionInformation, this); - m_serviceOnNotificationVisionTopic = m_n.advertiseService("vision_config/activate_publishing_of_vision_topic", &VisionConfigServices::OnNotificationVisionTopic, this); - m_serviceDoSensorFocusAction = m_n.advertiseService("vision_config/do_sensor_focus_action", &VisionConfigServices::DoSensorFocusAction, this); - m_serviceGetIntrinsicParameters = m_n.advertiseService("vision_config/get_intrinsic_parameters", &VisionConfigServices::GetIntrinsicParameters, this); - m_serviceGetIntrinsicParametersProfile = m_n.advertiseService("vision_config/get_intrinsic_parameters_profile", &VisionConfigServices::GetIntrinsicParametersProfile, this); - m_serviceSetIntrinsicParameters = m_n.advertiseService("vision_config/set_intrinsic_parameters", &VisionConfigServices::SetIntrinsicParameters, this); - m_serviceGetExtrinsicParameters = m_n.advertiseService("vision_config/get_extrinsic_parameters", &VisionConfigServices::GetExtrinsicParameters, this); - m_serviceSetExtrinsicParameters = m_n.advertiseService("vision_config/set_extrinsic_parameters", &VisionConfigServices::SetExtrinsicParameters, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("vision_config/set_device_id", &VisionConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("vision_config/set_api_options", &VisionConfigRobotServices::SetApiOptions, this); + + m_serviceSetSensorSettings = m_node_handle.advertiseService("vision_config/set_sensor_settings", &VisionConfigRobotServices::SetSensorSettings, this); + m_serviceGetSensorSettings = m_node_handle.advertiseService("vision_config/get_sensor_settings", &VisionConfigRobotServices::GetSensorSettings, this); + m_serviceGetOptionValue = m_node_handle.advertiseService("vision_config/get_option_value", &VisionConfigRobotServices::GetOptionValue, this); + m_serviceSetOptionValue = m_node_handle.advertiseService("vision_config/set_option_value", &VisionConfigRobotServices::SetOptionValue, this); + m_serviceGetOptionInformation = m_node_handle.advertiseService("vision_config/get_option_information", &VisionConfigRobotServices::GetOptionInformation, this); + m_serviceOnNotificationVisionTopic = m_node_handle.advertiseService("vision_config/activate_publishing_of_vision_topic", &VisionConfigRobotServices::OnNotificationVisionTopic, this); + m_serviceDoSensorFocusAction = m_node_handle.advertiseService("vision_config/do_sensor_focus_action", &VisionConfigRobotServices::DoSensorFocusAction, this); + m_serviceGetIntrinsicParameters = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters", &VisionConfigRobotServices::GetIntrinsicParameters, this); + m_serviceGetIntrinsicParametersProfile = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters_profile", &VisionConfigRobotServices::GetIntrinsicParametersProfile, this); + m_serviceSetIntrinsicParameters = m_node_handle.advertiseService("vision_config/set_intrinsic_parameters", &VisionConfigRobotServices::SetIntrinsicParameters, this); + m_serviceGetExtrinsicParameters = m_node_handle.advertiseService("vision_config/get_extrinsic_parameters", &VisionConfigRobotServices::GetExtrinsicParameters, this); + m_serviceSetExtrinsicParameters = m_node_handle.advertiseService("vision_config/set_extrinsic_parameters", &VisionConfigRobotServices::SetExtrinsicParameters, this); } -bool VisionConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool VisionConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool VisionConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool VisionConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -85,7 +85,7 @@ bool VisionConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool VisionConfigServices::SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) +bool VisionConfigRobotServices::SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) { Kinova::Api::VisionConfig::SensorSettings input; @@ -117,7 +117,7 @@ bool VisionConfigServices::SetSensorSettings(kortex_driver::SetSensorSettings::R return true; } -bool VisionConfigServices::GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) +bool VisionConfigRobotServices::GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) { Kinova::Api::VisionConfig::SensorIdentifier input; @@ -152,7 +152,7 @@ bool VisionConfigServices::GetSensorSettings(kortex_driver::GetSensorSettings::R return true; } -bool VisionConfigServices::GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) +bool VisionConfigRobotServices::GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) { Kinova::Api::VisionConfig::OptionIdentifier input; @@ -187,7 +187,7 @@ bool VisionConfigServices::GetOptionValue(kortex_driver::GetOptionValue::Request return true; } -bool VisionConfigServices::SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) +bool VisionConfigRobotServices::SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) { Kinova::Api::VisionConfig::OptionValue input; @@ -219,7 +219,7 @@ bool VisionConfigServices::SetOptionValue(kortex_driver::SetOptionValue::Request return true; } -bool VisionConfigServices::GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) +bool VisionConfigRobotServices::GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) { Kinova::Api::VisionConfig::OptionIdentifier input; @@ -254,7 +254,7 @@ bool VisionConfigServices::GetOptionInformation(kortex_driver::GetOptionInformat return true; } -bool VisionConfigServices::OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) +bool VisionConfigRobotServices::OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -268,7 +268,7 @@ bool VisionConfigServices::OnNotificationVisionTopic(kortex_driver::OnNotificati try { - std::function< void (Kinova::Api::VisionConfig::VisionNotification) > callback = std::bind(&VisionConfigServices::cb_VisionTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::VisionConfig::VisionNotification) > callback = std::bind(&VisionConfigRobotServices::cb_VisionTopic, this, std::placeholders::_1); output = m_visionconfig->OnNotificationVisionTopic(callback, input, m_current_device_id); m_is_activated_VisionTopic = true; } @@ -293,14 +293,14 @@ bool VisionConfigServices::OnNotificationVisionTopic(kortex_driver::OnNotificati ToRosData(output, res.output); return true; } -void VisionConfigServices::cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) +void VisionConfigRobotServices::cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) { kortex_driver::VisionNotification ros_msg; ToRosData(notif, ros_msg); m_pub_VisionTopic.publish(ros_msg); } -bool VisionConfigServices::DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) +bool VisionConfigRobotServices::DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) { Kinova::Api::VisionConfig::SensorFocusAction input; @@ -332,7 +332,7 @@ bool VisionConfigServices::DoSensorFocusAction(kortex_driver::DoSensorFocusActio return true; } -bool VisionConfigServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) +bool VisionConfigRobotServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) { Kinova::Api::VisionConfig::SensorIdentifier input; @@ -367,7 +367,7 @@ bool VisionConfigServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicPar return true; } -bool VisionConfigServices::GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) +bool VisionConfigRobotServices::GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) { Kinova::Api::VisionConfig::IntrinsicProfileIdentifier input; @@ -402,7 +402,7 @@ bool VisionConfigServices::GetIntrinsicParametersProfile(kortex_driver::GetIntri return true; } -bool VisionConfigServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) +bool VisionConfigRobotServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) { Kinova::Api::VisionConfig::IntrinsicParameters input; @@ -434,7 +434,7 @@ bool VisionConfigServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicPar return true; } -bool VisionConfigServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) +bool VisionConfigRobotServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) { Kinova::Api::VisionConfig::ExtrinsicParameters output; @@ -467,7 +467,7 @@ bool VisionConfigServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicPar return true; } -bool VisionConfigServices::SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) +bool VisionConfigRobotServices::SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) { Kinova::Api::VisionConfig::ExtrinsicParameters input; diff --git a/kortex_driver/src/generated/simulation/actuatorconfig_services.cpp b/kortex_driver/src/generated/simulation/actuatorconfig_services.cpp new file mode 100644 index 00000000..224c14b5 --- /dev/null +++ b/kortex_driver/src/generated/simulation/actuatorconfig_services.cpp @@ -0,0 +1,386 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/actuatorconfig_services.h" + +ActuatorConfigSimulationServices::ActuatorConfigSimulationServices(ros::NodeHandle& node_handle): + IActuatorConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("actuator_config/set_device_id", &ActuatorConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("actuator_config/set_api_options", &ActuatorConfigSimulationServices::SetApiOptions, this); + + m_serviceGetAxisOffsets = m_node_handle.advertiseService("actuator_config/get_axis_offsets", &ActuatorConfigSimulationServices::GetAxisOffsets, this); + m_serviceSetAxisOffsets = m_node_handle.advertiseService("actuator_config/set_axis_offsets", &ActuatorConfigSimulationServices::SetAxisOffsets, this); + m_serviceSetTorqueOffset = m_node_handle.advertiseService("actuator_config/set_torque_offset", &ActuatorConfigSimulationServices::SetTorqueOffset, this); + m_serviceActuatorConfig_GetControlMode = m_node_handle.advertiseService("actuator_config/get_control_mode", &ActuatorConfigSimulationServices::ActuatorConfig_GetControlMode, this); + m_serviceSetControlMode = m_node_handle.advertiseService("actuator_config/set_control_mode", &ActuatorConfigSimulationServices::SetControlMode, this); + m_serviceGetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/get_activated_control_loop", &ActuatorConfigSimulationServices::GetActivatedControlLoop, this); + m_serviceSetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/set_activated_control_loop", &ActuatorConfigSimulationServices::SetActivatedControlLoop, this); + m_serviceGetControlLoopParameters = m_node_handle.advertiseService("actuator_config/get_control_loop_parameters", &ActuatorConfigSimulationServices::GetControlLoopParameters, this); + m_serviceSetControlLoopParameters = m_node_handle.advertiseService("actuator_config/set_control_loop_parameters", &ActuatorConfigSimulationServices::SetControlLoopParameters, this); + m_serviceSelectCustomData = m_node_handle.advertiseService("actuator_config/select_custom_data", &ActuatorConfigSimulationServices::SelectCustomData, this); + m_serviceGetSelectedCustomData = m_node_handle.advertiseService("actuator_config/get_selected_custom_data", &ActuatorConfigSimulationServices::GetSelectedCustomData, this); + m_serviceSetCommandMode = m_node_handle.advertiseService("actuator_config/set_command_mode", &ActuatorConfigSimulationServices::SetCommandMode, this); + m_serviceActuatorConfig_ClearFaults = m_node_handle.advertiseService("actuator_config/clear_faults", &ActuatorConfigSimulationServices::ActuatorConfig_ClearFaults, this); + m_serviceSetServoing = m_node_handle.advertiseService("actuator_config/set_servoing", &ActuatorConfigSimulationServices::SetServoing, this); + m_serviceMoveToPosition = m_node_handle.advertiseService("actuator_config/move_to_position", &ActuatorConfigSimulationServices::MoveToPosition, this); + m_serviceGetCommandMode = m_node_handle.advertiseService("actuator_config/get_command_mode", &ActuatorConfigSimulationServices::GetCommandMode, this); + m_serviceGetServoing = m_node_handle.advertiseService("actuator_config/get_servoing", &ActuatorConfigSimulationServices::GetServoing, this); + m_serviceGetTorqueOffset = m_node_handle.advertiseService("actuator_config/get_torque_offset", &ActuatorConfigSimulationServices::GetTorqueOffset, this); + m_serviceSetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/set_cogging_feedforward_mode", &ActuatorConfigSimulationServices::SetCoggingFeedforwardMode, this); + m_serviceGetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/get_cogging_feedforward_mode", &ActuatorConfigSimulationServices::GetCoggingFeedforwardMode, this); +} + +bool ActuatorConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool ActuatorConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool ActuatorConfigSimulationServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) +{ + + + if (GetAxisOffsetsHandler) + { + res = GetAxisOffsetsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_axis_offsets is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) +{ + + + if (SetAxisOffsetsHandler) + { + res = SetAxisOffsetsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_axis_offsets is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) +{ + + + if (SetTorqueOffsetHandler) + { + res = SetTorqueOffsetHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_torque_offset is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) +{ + + + if (ActuatorConfig_GetControlModeHandler) + { + res = ActuatorConfig_GetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) +{ + + + if (SetControlModeHandler) + { + res = SetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) +{ + + + if (GetActivatedControlLoopHandler) + { + res = GetActivatedControlLoopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_activated_control_loop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) +{ + + + if (SetActivatedControlLoopHandler) + { + res = SetActivatedControlLoopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_activated_control_loop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) +{ + + + if (GetControlLoopParametersHandler) + { + res = GetControlLoopParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_control_loop_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) +{ + + + if (SetControlLoopParametersHandler) + { + res = SetControlLoopParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_control_loop_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) +{ + + + if (SelectCustomDataHandler) + { + res = SelectCustomDataHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/select_custom_data is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) +{ + + + if (GetSelectedCustomDataHandler) + { + res = GetSelectedCustomDataHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_selected_custom_data is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) +{ + + + if (SetCommandModeHandler) + { + res = SetCommandModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_command_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) +{ + + + if (ActuatorConfig_ClearFaultsHandler) + { + res = ActuatorConfig_ClearFaultsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/clear_faults is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) +{ + + + if (SetServoingHandler) + { + res = SetServoingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_servoing is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) +{ + + + if (MoveToPositionHandler) + { + res = MoveToPositionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/move_to_position is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) +{ + + + if (GetCommandModeHandler) + { + res = GetCommandModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_command_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) +{ + + + if (GetServoingHandler) + { + res = GetServoingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_servoing is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) +{ + + + if (GetTorqueOffsetHandler) + { + res = GetTorqueOffsetHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_torque_offset is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) +{ + + + if (SetCoggingFeedforwardModeHandler) + { + res = SetCoggingFeedforwardModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_cogging_feedforward_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) +{ + + + if (GetCoggingFeedforwardModeHandler) + { + res = GetCoggingFeedforwardModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_cogging_feedforward_mode is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/base_services.cpp b/kortex_driver/src/generated/simulation/base_services.cpp new file mode 100644 index 00000000..a8e3589f --- /dev/null +++ b/kortex_driver/src/generated/simulation/base_services.cpp @@ -0,0 +1,2504 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/base_services.h" + +BaseSimulationServices::BaseSimulationServices(ros::NodeHandle& node_handle): + IBaseServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ConfigurationChangeTopic = m_node_handle.advertise("configuration_change_topic", 1000); + m_is_activated_ConfigurationChangeTopic = false; + m_pub_MappingInfoTopic = m_node_handle.advertise("mapping_info_topic", 1000); + m_is_activated_MappingInfoTopic = false; + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); + m_is_activated_ControlModeTopic = false; + m_pub_OperatingModeTopic = m_node_handle.advertise("operating_mode_topic", 1000); + m_is_activated_OperatingModeTopic = false; + m_pub_SequenceInfoTopic = m_node_handle.advertise("sequence_info_topic", 1000); + m_is_activated_SequenceInfoTopic = false; + m_pub_ProtectionZoneTopic = m_node_handle.advertise("protection_zone_topic", 1000); + m_is_activated_ProtectionZoneTopic = false; + m_pub_UserTopic = m_node_handle.advertise("user_topic", 1000); + m_is_activated_UserTopic = false; + m_pub_ControllerTopic = m_node_handle.advertise("controller_topic", 1000); + m_is_activated_ControllerTopic = false; + m_pub_ActionTopic = m_node_handle.advertise("action_topic", 1000); + m_is_activated_ActionTopic = false; + m_pub_RobotEventTopic = m_node_handle.advertise("robot_event_topic", 1000); + m_is_activated_RobotEventTopic = false; + m_pub_ServoingModeTopic = m_node_handle.advertise("servoing_mode_topic", 1000); + m_is_activated_ServoingModeTopic = false; + m_pub_FactoryTopic = m_node_handle.advertise("factory_topic", 1000); + m_is_activated_FactoryTopic = false; + m_pub_NetworkTopic = m_node_handle.advertise("network_topic", 1000); + m_is_activated_NetworkTopic = false; + m_pub_ArmStateTopic = m_node_handle.advertise("arm_state_topic", 1000); + m_is_activated_ArmStateTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("base/set_device_id", &BaseSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("base/set_api_options", &BaseSimulationServices::SetApiOptions, this); + + m_serviceCreateUserProfile = m_node_handle.advertiseService("base/create_user_profile", &BaseSimulationServices::CreateUserProfile, this); + m_serviceUpdateUserProfile = m_node_handle.advertiseService("base/update_user_profile", &BaseSimulationServices::UpdateUserProfile, this); + m_serviceReadUserProfile = m_node_handle.advertiseService("base/read_user_profile", &BaseSimulationServices::ReadUserProfile, this); + m_serviceDeleteUserProfile = m_node_handle.advertiseService("base/delete_user_profile", &BaseSimulationServices::DeleteUserProfile, this); + m_serviceReadAllUserProfiles = m_node_handle.advertiseService("base/read_all_user_profiles", &BaseSimulationServices::ReadAllUserProfiles, this); + m_serviceReadAllUsers = m_node_handle.advertiseService("base/read_all_users", &BaseSimulationServices::ReadAllUsers, this); + m_serviceChangePassword = m_node_handle.advertiseService("base/change_password", &BaseSimulationServices::ChangePassword, this); + m_serviceCreateSequence = m_node_handle.advertiseService("base/create_sequence", &BaseSimulationServices::CreateSequence, this); + m_serviceUpdateSequence = m_node_handle.advertiseService("base/update_sequence", &BaseSimulationServices::UpdateSequence, this); + m_serviceReadSequence = m_node_handle.advertiseService("base/read_sequence", &BaseSimulationServices::ReadSequence, this); + m_serviceDeleteSequence = m_node_handle.advertiseService("base/delete_sequence", &BaseSimulationServices::DeleteSequence, this); + m_serviceReadAllSequences = m_node_handle.advertiseService("base/read_all_sequences", &BaseSimulationServices::ReadAllSequences, this); + m_servicePlaySequence = m_node_handle.advertiseService("base/play_sequence", &BaseSimulationServices::PlaySequence, this); + m_servicePlayAdvancedSequence = m_node_handle.advertiseService("base/play_advanced_sequence", &BaseSimulationServices::PlayAdvancedSequence, this); + m_serviceStopSequence = m_node_handle.advertiseService("base/stop_sequence", &BaseSimulationServices::StopSequence, this); + m_servicePauseSequence = m_node_handle.advertiseService("base/pause_sequence", &BaseSimulationServices::PauseSequence, this); + m_serviceResumeSequence = m_node_handle.advertiseService("base/resume_sequence", &BaseSimulationServices::ResumeSequence, this); + m_serviceCreateProtectionZone = m_node_handle.advertiseService("base/create_protection_zone", &BaseSimulationServices::CreateProtectionZone, this); + m_serviceUpdateProtectionZone = m_node_handle.advertiseService("base/update_protection_zone", &BaseSimulationServices::UpdateProtectionZone, this); + m_serviceReadProtectionZone = m_node_handle.advertiseService("base/read_protection_zone", &BaseSimulationServices::ReadProtectionZone, this); + m_serviceDeleteProtectionZone = m_node_handle.advertiseService("base/delete_protection_zone", &BaseSimulationServices::DeleteProtectionZone, this); + m_serviceReadAllProtectionZones = m_node_handle.advertiseService("base/read_all_protection_zones", &BaseSimulationServices::ReadAllProtectionZones, this); + m_serviceCreateMapping = m_node_handle.advertiseService("base/create_mapping", &BaseSimulationServices::CreateMapping, this); + m_serviceReadMapping = m_node_handle.advertiseService("base/read_mapping", &BaseSimulationServices::ReadMapping, this); + m_serviceUpdateMapping = m_node_handle.advertiseService("base/update_mapping", &BaseSimulationServices::UpdateMapping, this); + m_serviceDeleteMapping = m_node_handle.advertiseService("base/delete_mapping", &BaseSimulationServices::DeleteMapping, this); + m_serviceReadAllMappings = m_node_handle.advertiseService("base/read_all_mappings", &BaseSimulationServices::ReadAllMappings, this); + m_serviceCreateMap = m_node_handle.advertiseService("base/create_map", &BaseSimulationServices::CreateMap, this); + m_serviceReadMap = m_node_handle.advertiseService("base/read_map", &BaseSimulationServices::ReadMap, this); + m_serviceUpdateMap = m_node_handle.advertiseService("base/update_map", &BaseSimulationServices::UpdateMap, this); + m_serviceDeleteMap = m_node_handle.advertiseService("base/delete_map", &BaseSimulationServices::DeleteMap, this); + m_serviceReadAllMaps = m_node_handle.advertiseService("base/read_all_maps", &BaseSimulationServices::ReadAllMaps, this); + m_serviceActivateMap = m_node_handle.advertiseService("base/activate_map", &BaseSimulationServices::ActivateMap, this); + m_serviceCreateAction = m_node_handle.advertiseService("base/create_action", &BaseSimulationServices::CreateAction, this); + m_serviceReadAction = m_node_handle.advertiseService("base/read_action", &BaseSimulationServices::ReadAction, this); + m_serviceReadAllActions = m_node_handle.advertiseService("base/read_all_actions", &BaseSimulationServices::ReadAllActions, this); + m_serviceDeleteAction = m_node_handle.advertiseService("base/delete_action", &BaseSimulationServices::DeleteAction, this); + m_serviceUpdateAction = m_node_handle.advertiseService("base/update_action", &BaseSimulationServices::UpdateAction, this); + m_serviceExecuteActionFromReference = m_node_handle.advertiseService("base/execute_action_from_reference", &BaseSimulationServices::ExecuteActionFromReference, this); + m_serviceExecuteAction = m_node_handle.advertiseService("base/execute_action", &BaseSimulationServices::ExecuteAction, this); + m_servicePauseAction = m_node_handle.advertiseService("base/pause_action", &BaseSimulationServices::PauseAction, this); + m_serviceStopAction = m_node_handle.advertiseService("base/stop_action", &BaseSimulationServices::StopAction, this); + m_serviceResumeAction = m_node_handle.advertiseService("base/resume_action", &BaseSimulationServices::ResumeAction, this); + m_serviceGetIPv4Configuration = m_node_handle.advertiseService("base/get_i_pv4_configuration", &BaseSimulationServices::GetIPv4Configuration, this); + m_serviceSetIPv4Configuration = m_node_handle.advertiseService("base/set_i_pv4_configuration", &BaseSimulationServices::SetIPv4Configuration, this); + m_serviceSetCommunicationInterfaceEnable = m_node_handle.advertiseService("base/set_communication_interface_enable", &BaseSimulationServices::SetCommunicationInterfaceEnable, this); + m_serviceIsCommunicationInterfaceEnable = m_node_handle.advertiseService("base/is_communication_interface_enable", &BaseSimulationServices::IsCommunicationInterfaceEnable, this); + m_serviceGetAvailableWifi = m_node_handle.advertiseService("base/get_available_wifi", &BaseSimulationServices::GetAvailableWifi, this); + m_serviceGetWifiInformation = m_node_handle.advertiseService("base/get_wifi_information", &BaseSimulationServices::GetWifiInformation, this); + m_serviceAddWifiConfiguration = m_node_handle.advertiseService("base/add_wifi_configuration", &BaseSimulationServices::AddWifiConfiguration, this); + m_serviceDeleteWifiConfiguration = m_node_handle.advertiseService("base/delete_wifi_configuration", &BaseSimulationServices::DeleteWifiConfiguration, this); + m_serviceGetAllConfiguredWifis = m_node_handle.advertiseService("base/get_all_configured_wifis", &BaseSimulationServices::GetAllConfiguredWifis, this); + m_serviceConnectWifi = m_node_handle.advertiseService("base/connect_wifi", &BaseSimulationServices::ConnectWifi, this); + m_serviceDisconnectWifi = m_node_handle.advertiseService("base/disconnect_wifi", &BaseSimulationServices::DisconnectWifi, this); + m_serviceGetConnectedWifiInformation = m_node_handle.advertiseService("base/get_connected_wifi_information", &BaseSimulationServices::GetConnectedWifiInformation, this); + m_serviceBase_Unsubscribe = m_node_handle.advertiseService("base/unsubscribe", &BaseSimulationServices::Base_Unsubscribe, this); + m_serviceOnNotificationConfigurationChangeTopic = m_node_handle.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseSimulationServices::OnNotificationConfigurationChangeTopic, this); + m_serviceOnNotificationMappingInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseSimulationServices::OnNotificationMappingInfoTopic, this); + m_serviceOnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseSimulationServices::OnNotificationControlModeTopic, this); + m_serviceOnNotificationOperatingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseSimulationServices::OnNotificationOperatingModeTopic, this); + m_serviceOnNotificationSequenceInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseSimulationServices::OnNotificationSequenceInfoTopic, this); + m_serviceOnNotificationProtectionZoneTopic = m_node_handle.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseSimulationServices::OnNotificationProtectionZoneTopic, this); + m_serviceOnNotificationUserTopic = m_node_handle.advertiseService("base/activate_publishing_of_user_topic", &BaseSimulationServices::OnNotificationUserTopic, this); + m_serviceOnNotificationControllerTopic = m_node_handle.advertiseService("base/activate_publishing_of_controller_topic", &BaseSimulationServices::OnNotificationControllerTopic, this); + m_serviceOnNotificationActionTopic = m_node_handle.advertiseService("base/activate_publishing_of_action_topic", &BaseSimulationServices::OnNotificationActionTopic, this); + m_serviceOnNotificationRobotEventTopic = m_node_handle.advertiseService("base/activate_publishing_of_robot_event_topic", &BaseSimulationServices::OnNotificationRobotEventTopic, this); + m_servicePlayCartesianTrajectory = m_node_handle.advertiseService("base/play_cartesian_trajectory", &BaseSimulationServices::PlayCartesianTrajectory, this); + m_servicePlayCartesianTrajectoryPosition = m_node_handle.advertiseService("base/play_cartesian_trajectory_position", &BaseSimulationServices::PlayCartesianTrajectoryPosition, this); + m_servicePlayCartesianTrajectoryOrientation = m_node_handle.advertiseService("base/play_cartesian_trajectory_orientation", &BaseSimulationServices::PlayCartesianTrajectoryOrientation, this); + m_serviceStop = m_node_handle.advertiseService("base/stop", &BaseSimulationServices::Stop, this); + m_serviceGetMeasuredCartesianPose = m_node_handle.advertiseService("base/get_measured_cartesian_pose", &BaseSimulationServices::GetMeasuredCartesianPose, this); + m_serviceSendWrenchCommand = m_node_handle.advertiseService("base/send_wrench_command", &BaseSimulationServices::SendWrenchCommand, this); + m_serviceSendWrenchJoystickCommand = m_node_handle.advertiseService("base/send_wrench_joystick_command", &BaseSimulationServices::SendWrenchJoystickCommand, this); + m_serviceSendTwistJoystickCommand = m_node_handle.advertiseService("base/send_twist_joystick_command", &BaseSimulationServices::SendTwistJoystickCommand, this); + m_serviceSendTwistCommand = m_node_handle.advertiseService("base/send_twist_command", &BaseSimulationServices::SendTwistCommand, this); + m_servicePlayJointTrajectory = m_node_handle.advertiseService("base/play_joint_trajectory", &BaseSimulationServices::PlayJointTrajectory, this); + m_servicePlaySelectedJointTrajectory = m_node_handle.advertiseService("base/play_selected_joint_trajectory", &BaseSimulationServices::PlaySelectedJointTrajectory, this); + m_serviceGetMeasuredJointAngles = m_node_handle.advertiseService("base/get_measured_joint_angles", &BaseSimulationServices::GetMeasuredJointAngles, this); + m_serviceSendJointSpeedsCommand = m_node_handle.advertiseService("base/send_joint_speeds_command", &BaseSimulationServices::SendJointSpeedsCommand, this); + m_serviceSendSelectedJointSpeedCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_command", &BaseSimulationServices::SendSelectedJointSpeedCommand, this); + m_serviceSendGripperCommand = m_node_handle.advertiseService("base/send_gripper_command", &BaseSimulationServices::SendGripperCommand, this); + m_serviceGetMeasuredGripperMovement = m_node_handle.advertiseService("base/get_measured_gripper_movement", &BaseSimulationServices::GetMeasuredGripperMovement, this); + m_serviceSetAdmittance = m_node_handle.advertiseService("base/set_admittance", &BaseSimulationServices::SetAdmittance, this); + m_serviceSetOperatingMode = m_node_handle.advertiseService("base/set_operating_mode", &BaseSimulationServices::SetOperatingMode, this); + m_serviceApplyEmergencyStop = m_node_handle.advertiseService("base/apply_emergency_stop", &BaseSimulationServices::ApplyEmergencyStop, this); + m_serviceBase_ClearFaults = m_node_handle.advertiseService("base/clear_faults", &BaseSimulationServices::Base_ClearFaults, this); + m_serviceBase_GetControlMode = m_node_handle.advertiseService("base/get_control_mode", &BaseSimulationServices::Base_GetControlMode, this); + m_serviceGetOperatingMode = m_node_handle.advertiseService("base/get_operating_mode", &BaseSimulationServices::GetOperatingMode, this); + m_serviceSetServoingMode = m_node_handle.advertiseService("base/set_servoing_mode", &BaseSimulationServices::SetServoingMode, this); + m_serviceGetServoingMode = m_node_handle.advertiseService("base/get_servoing_mode", &BaseSimulationServices::GetServoingMode, this); + m_serviceOnNotificationServoingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_servoing_mode_topic", &BaseSimulationServices::OnNotificationServoingModeTopic, this); + m_serviceRestoreFactorySettings = m_node_handle.advertiseService("base/restore_factory_settings", &BaseSimulationServices::RestoreFactorySettings, this); + m_serviceOnNotificationFactoryTopic = m_node_handle.advertiseService("base/activate_publishing_of_factory_topic", &BaseSimulationServices::OnNotificationFactoryTopic, this); + m_serviceGetAllConnectedControllers = m_node_handle.advertiseService("base/get_all_connected_controllers", &BaseSimulationServices::GetAllConnectedControllers, this); + m_serviceGetControllerState = m_node_handle.advertiseService("base/get_controller_state", &BaseSimulationServices::GetControllerState, this); + m_serviceGetActuatorCount = m_node_handle.advertiseService("base/get_actuator_count", &BaseSimulationServices::GetActuatorCount, this); + m_serviceStartWifiScan = m_node_handle.advertiseService("base/start_wifi_scan", &BaseSimulationServices::StartWifiScan, this); + m_serviceGetConfiguredWifi = m_node_handle.advertiseService("base/get_configured_wifi", &BaseSimulationServices::GetConfiguredWifi, this); + m_serviceOnNotificationNetworkTopic = m_node_handle.advertiseService("base/activate_publishing_of_network_topic", &BaseSimulationServices::OnNotificationNetworkTopic, this); + m_serviceGetArmState = m_node_handle.advertiseService("base/get_arm_state", &BaseSimulationServices::GetArmState, this); + m_serviceOnNotificationArmStateTopic = m_node_handle.advertiseService("base/activate_publishing_of_arm_state_topic", &BaseSimulationServices::OnNotificationArmStateTopic, this); + m_serviceGetIPv4Information = m_node_handle.advertiseService("base/get_i_pv4_information", &BaseSimulationServices::GetIPv4Information, this); + m_serviceSetWifiCountryCode = m_node_handle.advertiseService("base/set_wifi_country_code", &BaseSimulationServices::SetWifiCountryCode, this); + m_serviceGetWifiCountryCode = m_node_handle.advertiseService("base/get_wifi_country_code", &BaseSimulationServices::GetWifiCountryCode, this); + m_serviceBase_SetCapSenseConfig = m_node_handle.advertiseService("base/set_cap_sense_config", &BaseSimulationServices::Base_SetCapSenseConfig, this); + m_serviceBase_GetCapSenseConfig = m_node_handle.advertiseService("base/get_cap_sense_config", &BaseSimulationServices::Base_GetCapSenseConfig, this); + m_serviceGetAllJointsSpeedHardLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_hard_limitation", &BaseSimulationServices::GetAllJointsSpeedHardLimitation, this); + m_serviceGetAllJointsTorqueHardLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_hard_limitation", &BaseSimulationServices::GetAllJointsTorqueHardLimitation, this); + m_serviceGetTwistHardLimitation = m_node_handle.advertiseService("base/get_twist_hard_limitation", &BaseSimulationServices::GetTwistHardLimitation, this); + m_serviceGetWrenchHardLimitation = m_node_handle.advertiseService("base/get_wrench_hard_limitation", &BaseSimulationServices::GetWrenchHardLimitation, this); + m_serviceSendJointSpeedsJoystickCommand = m_node_handle.advertiseService("base/send_joint_speeds_joystick_command", &BaseSimulationServices::SendJointSpeedsJoystickCommand, this); + m_serviceSendSelectedJointSpeedJoystickCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_joystick_command", &BaseSimulationServices::SendSelectedJointSpeedJoystickCommand, this); + m_serviceEnableBridge = m_node_handle.advertiseService("base/enable_bridge", &BaseSimulationServices::EnableBridge, this); + m_serviceDisableBridge = m_node_handle.advertiseService("base/disable_bridge", &BaseSimulationServices::DisableBridge, this); + m_serviceGetBridgeList = m_node_handle.advertiseService("base/get_bridge_list", &BaseSimulationServices::GetBridgeList, this); + m_serviceGetBridgeConfig = m_node_handle.advertiseService("base/get_bridge_config", &BaseSimulationServices::GetBridgeConfig, this); + m_servicePlayPreComputedJointTrajectory = m_node_handle.advertiseService("base/play_pre_computed_joint_trajectory", &BaseSimulationServices::PlayPreComputedJointTrajectory, this); + m_serviceGetProductConfiguration = m_node_handle.advertiseService("base/get_product_configuration", &BaseSimulationServices::GetProductConfiguration, this); + m_serviceUpdateEndEffectorTypeConfiguration = m_node_handle.advertiseService("base/update_end_effector_type_configuration", &BaseSimulationServices::UpdateEndEffectorTypeConfiguration, this); + m_serviceRestoreFactoryProductConfiguration = m_node_handle.advertiseService("base/restore_factory_product_configuration", &BaseSimulationServices::RestoreFactoryProductConfiguration, this); + m_serviceGetTrajectoryErrorReport = m_node_handle.advertiseService("base/get_trajectory_error_report", &BaseSimulationServices::GetTrajectoryErrorReport, this); + m_serviceGetAllJointsSpeedSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_soft_limitation", &BaseSimulationServices::GetAllJointsSpeedSoftLimitation, this); + m_serviceGetAllJointsTorqueSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_soft_limitation", &BaseSimulationServices::GetAllJointsTorqueSoftLimitation, this); + m_serviceGetTwistSoftLimitation = m_node_handle.advertiseService("base/get_twist_soft_limitation", &BaseSimulationServices::GetTwistSoftLimitation, this); + m_serviceGetWrenchSoftLimitation = m_node_handle.advertiseService("base/get_wrench_soft_limitation", &BaseSimulationServices::GetWrenchSoftLimitation, this); + m_serviceSetControllerConfigurationMode = m_node_handle.advertiseService("base/set_controller_configuration_mode", &BaseSimulationServices::SetControllerConfigurationMode, this); + m_serviceGetControllerConfigurationMode = m_node_handle.advertiseService("base/get_controller_configuration_mode", &BaseSimulationServices::GetControllerConfigurationMode, this); + m_serviceStartTeaching = m_node_handle.advertiseService("base/start_teaching", &BaseSimulationServices::StartTeaching, this); + m_serviceStopTeaching = m_node_handle.advertiseService("base/stop_teaching", &BaseSimulationServices::StopTeaching, this); + m_serviceAddSequenceTasks = m_node_handle.advertiseService("base/add_sequence_tasks", &BaseSimulationServices::AddSequenceTasks, this); + m_serviceUpdateSequenceTask = m_node_handle.advertiseService("base/update_sequence_task", &BaseSimulationServices::UpdateSequenceTask, this); + m_serviceSwapSequenceTasks = m_node_handle.advertiseService("base/swap_sequence_tasks", &BaseSimulationServices::SwapSequenceTasks, this); + m_serviceReadSequenceTask = m_node_handle.advertiseService("base/read_sequence_task", &BaseSimulationServices::ReadSequenceTask, this); + m_serviceReadAllSequenceTasks = m_node_handle.advertiseService("base/read_all_sequence_tasks", &BaseSimulationServices::ReadAllSequenceTasks, this); + m_serviceDeleteSequenceTask = m_node_handle.advertiseService("base/delete_sequence_task", &BaseSimulationServices::DeleteSequenceTask, this); + m_serviceDeleteAllSequenceTasks = m_node_handle.advertiseService("base/delete_all_sequence_tasks", &BaseSimulationServices::DeleteAllSequenceTasks, this); + m_serviceTakeSnapshot = m_node_handle.advertiseService("base/take_snapshot", &BaseSimulationServices::TakeSnapshot, this); + m_serviceGetFirmwareBundleVersions = m_node_handle.advertiseService("base/get_firmware_bundle_versions", &BaseSimulationServices::GetFirmwareBundleVersions, this); + m_serviceMoveSequenceTask = m_node_handle.advertiseService("base/move_sequence_task", &BaseSimulationServices::MoveSequenceTask, this); + m_serviceDuplicateMapping = m_node_handle.advertiseService("base/duplicate_mapping", &BaseSimulationServices::DuplicateMapping, this); + m_serviceDuplicateMap = m_node_handle.advertiseService("base/duplicate_map", &BaseSimulationServices::DuplicateMap, this); + m_serviceSetControllerConfiguration = m_node_handle.advertiseService("base/set_controller_configuration", &BaseSimulationServices::SetControllerConfiguration, this); + m_serviceGetControllerConfiguration = m_node_handle.advertiseService("base/get_controller_configuration", &BaseSimulationServices::GetControllerConfiguration, this); + m_serviceGetAllControllerConfigurations = m_node_handle.advertiseService("base/get_all_controller_configurations", &BaseSimulationServices::GetAllControllerConfigurations, this); +} + +bool BaseSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool BaseSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool BaseSimulationServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) +{ + + + if (CreateUserProfileHandler) + { + res = CreateUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) +{ + + + if (UpdateUserProfileHandler) + { + res = UpdateUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) +{ + + + if (ReadUserProfileHandler) + { + res = ReadUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) +{ + + + if (DeleteUserProfileHandler) + { + res = DeleteUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) +{ + + + if (ReadAllUserProfilesHandler) + { + res = ReadAllUserProfilesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_user_profiles is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) +{ + + + if (ReadAllUsersHandler) + { + res = ReadAllUsersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_users is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) +{ + + + if (ChangePasswordHandler) + { + res = ChangePasswordHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/change_password is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) +{ + + + if (CreateSequenceHandler) + { + res = CreateSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) +{ + + + if (UpdateSequenceHandler) + { + res = UpdateSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) +{ + + + if (ReadSequenceHandler) + { + res = ReadSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) +{ + + + if (DeleteSequenceHandler) + { + res = DeleteSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) +{ + + + if (ReadAllSequencesHandler) + { + res = ReadAllSequencesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_sequences is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) +{ + + + if (PlaySequenceHandler) + { + res = PlaySequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) +{ + + + if (PlayAdvancedSequenceHandler) + { + res = PlayAdvancedSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_advanced_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) +{ + + + if (StopSequenceHandler) + { + res = StopSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) +{ + + + if (PauseSequenceHandler) + { + res = PauseSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/pause_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) +{ + + + if (ResumeSequenceHandler) + { + res = ResumeSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/resume_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) +{ + + + if (CreateProtectionZoneHandler) + { + res = CreateProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) +{ + + + if (UpdateProtectionZoneHandler) + { + res = UpdateProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) +{ + + + if (ReadProtectionZoneHandler) + { + res = ReadProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) +{ + + + if (DeleteProtectionZoneHandler) + { + res = DeleteProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) +{ + + + if (ReadAllProtectionZonesHandler) + { + res = ReadAllProtectionZonesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_protection_zones is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) +{ + + + if (CreateMappingHandler) + { + res = CreateMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) +{ + + + if (ReadMappingHandler) + { + res = ReadMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) +{ + + + if (UpdateMappingHandler) + { + res = UpdateMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) +{ + + + if (DeleteMappingHandler) + { + res = DeleteMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) +{ + + + if (ReadAllMappingsHandler) + { + res = ReadAllMappingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_mappings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) +{ + + + if (CreateMapHandler) + { + res = CreateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) +{ + + + if (ReadMapHandler) + { + res = ReadMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) +{ + + + if (UpdateMapHandler) + { + res = UpdateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) +{ + + + if (DeleteMapHandler) + { + res = DeleteMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) +{ + + + if (ReadAllMapsHandler) + { + res = ReadAllMapsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_maps is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) +{ + + + if (ActivateMapHandler) + { + res = ActivateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) +{ + + + if (CreateActionHandler) + { + res = CreateActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) +{ + + + if (ReadActionHandler) + { + res = ReadActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) +{ + + + if (ReadAllActionsHandler) + { + res = ReadAllActionsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_actions is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) +{ + + + if (DeleteActionHandler) + { + res = DeleteActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) +{ + + + if (UpdateActionHandler) + { + res = UpdateActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) +{ + + + if (ExecuteActionFromReferenceHandler) + { + res = ExecuteActionFromReferenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/execute_action_from_reference is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) +{ + + + if (ExecuteActionHandler) + { + res = ExecuteActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/execute_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) +{ + + + if (PauseActionHandler) + { + res = PauseActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/pause_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) +{ + + + if (StopActionHandler) + { + res = StopActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) +{ + + + if (ResumeActionHandler) + { + res = ResumeActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/resume_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) +{ + + + if (GetIPv4ConfigurationHandler) + { + res = GetIPv4ConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_i_pv4_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) +{ + + + if (SetIPv4ConfigurationHandler) + { + res = SetIPv4ConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_i_pv4_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) +{ + + + if (SetCommunicationInterfaceEnableHandler) + { + res = SetCommunicationInterfaceEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_communication_interface_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) +{ + + + if (IsCommunicationInterfaceEnableHandler) + { + res = IsCommunicationInterfaceEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/is_communication_interface_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) +{ + + + if (GetAvailableWifiHandler) + { + res = GetAvailableWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_available_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) +{ + + + if (GetWifiInformationHandler) + { + res = GetWifiInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wifi_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) +{ + + + if (AddWifiConfigurationHandler) + { + res = AddWifiConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/add_wifi_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) +{ + + + if (DeleteWifiConfigurationHandler) + { + res = DeleteWifiConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_wifi_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) +{ + + + if (GetAllConfiguredWifisHandler) + { + res = GetAllConfiguredWifisHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_configured_wifis is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) +{ + + + if (ConnectWifiHandler) + { + res = ConnectWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/connect_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) +{ + + + if (DisconnectWifiHandler) + { + res = DisconnectWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/disconnect_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) +{ + + + if (GetConnectedWifiInformationHandler) + { + res = GetConnectedWifiInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_connected_wifi_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) +{ + + + if (Base_UnsubscribeHandler) + { + res = Base_UnsubscribeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/unsubscribe is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) +{ + + m_is_activated_ConfigurationChangeTopic = true; + + if (OnNotificationConfigurationChangeTopicHandler) + { + res = OnNotificationConfigurationChangeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_configuration_change_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) +{ + kortex_driver::ConfigurationChangeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ConfigurationChangeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) +{ + + m_is_activated_MappingInfoTopic = true; + + if (OnNotificationMappingInfoTopicHandler) + { + res = OnNotificationMappingInfoTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_mapping_info_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) +{ + kortex_driver::MappingInfoNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_MappingInfoTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) +{ + + m_is_activated_ControlModeTopic = true; + + if (OnNotificationControlModeTopicHandler) + { + res = OnNotificationControlModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_control_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) +{ + kortex_driver::ControlModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlModeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) +{ + + m_is_activated_OperatingModeTopic = true; + + if (OnNotificationOperatingModeTopicHandler) + { + res = OnNotificationOperatingModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_operating_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) +{ + kortex_driver::OperatingModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_OperatingModeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) +{ + + m_is_activated_SequenceInfoTopic = true; + + if (OnNotificationSequenceInfoTopicHandler) + { + res = OnNotificationSequenceInfoTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_sequence_info_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) +{ + kortex_driver::SequenceInfoNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_SequenceInfoTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) +{ + + m_is_activated_ProtectionZoneTopic = true; + + if (OnNotificationProtectionZoneTopicHandler) + { + res = OnNotificationProtectionZoneTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_protection_zone_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) +{ + kortex_driver::ProtectionZoneNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ProtectionZoneTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) +{ + + m_is_activated_UserTopic = true; + + if (OnNotificationUserTopicHandler) + { + res = OnNotificationUserTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_user_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_UserTopic(Kinova::Api::Base::UserNotification notif) +{ + kortex_driver::UserNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_UserTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) +{ + + m_is_activated_ControllerTopic = true; + + if (OnNotificationControllerTopicHandler) + { + res = OnNotificationControllerTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_controller_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) +{ + kortex_driver::ControllerNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControllerTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) +{ + + m_is_activated_ActionTopic = true; + + if (OnNotificationActionTopicHandler) + { + res = OnNotificationActionTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_action_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) +{ + kortex_driver::ActionNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ActionTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) +{ + + m_is_activated_RobotEventTopic = true; + + if (OnNotificationRobotEventTopicHandler) + { + res = OnNotificationRobotEventTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_robot_event_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) +{ + kortex_driver::RobotEventNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_RobotEventTopic.publish(ros_msg); +} + +bool BaseSimulationServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) +{ + + + if (PlayCartesianTrajectoryHandler) + { + res = PlayCartesianTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_cartesian_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) +{ + + + if (PlayCartesianTrajectoryPositionHandler) + { + res = PlayCartesianTrajectoryPositionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_cartesian_trajectory_position is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) +{ + + + if (PlayCartesianTrajectoryOrientationHandler) + { + res = PlayCartesianTrajectoryOrientationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_cartesian_trajectory_orientation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) +{ + + + if (StopHandler) + { + res = StopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) +{ + + + if (GetMeasuredCartesianPoseHandler) + { + res = GetMeasuredCartesianPoseHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_measured_cartesian_pose is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) +{ + + + if (SendWrenchCommandHandler) + { + res = SendWrenchCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_wrench_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) +{ + + + if (SendWrenchJoystickCommandHandler) + { + res = SendWrenchJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_wrench_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) +{ + + + if (SendTwistJoystickCommandHandler) + { + res = SendTwistJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_twist_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) +{ + + + if (SendTwistCommandHandler) + { + res = SendTwistCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_twist_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) +{ + + + if (PlayJointTrajectoryHandler) + { + res = PlayJointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_joint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) +{ + + + if (PlaySelectedJointTrajectoryHandler) + { + res = PlaySelectedJointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_selected_joint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) +{ + + + if (GetMeasuredJointAnglesHandler) + { + res = GetMeasuredJointAnglesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_measured_joint_angles is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) +{ + + + if (SendJointSpeedsCommandHandler) + { + res = SendJointSpeedsCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_joint_speeds_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) +{ + + + if (SendSelectedJointSpeedCommandHandler) + { + res = SendSelectedJointSpeedCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_selected_joint_speed_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) +{ + + + if (SendGripperCommandHandler) + { + res = SendGripperCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_gripper_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) +{ + + + if (GetMeasuredGripperMovementHandler) + { + res = GetMeasuredGripperMovementHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_measured_gripper_movement is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) +{ + + + if (SetAdmittanceHandler) + { + res = SetAdmittanceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_admittance is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) +{ + + + if (SetOperatingModeHandler) + { + res = SetOperatingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_operating_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) +{ + + + if (ApplyEmergencyStopHandler) + { + res = ApplyEmergencyStopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/apply_emergency_stop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) +{ + + + if (Base_ClearFaultsHandler) + { + res = Base_ClearFaultsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/clear_faults is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) +{ + + + if (Base_GetControlModeHandler) + { + res = Base_GetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) +{ + + + if (GetOperatingModeHandler) + { + res = GetOperatingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_operating_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) +{ + + + if (SetServoingModeHandler) + { + res = SetServoingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_servoing_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) +{ + + + if (GetServoingModeHandler) + { + res = GetServoingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_servoing_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) +{ + + m_is_activated_ServoingModeTopic = true; + + if (OnNotificationServoingModeTopicHandler) + { + res = OnNotificationServoingModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_servoing_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) +{ + kortex_driver::ServoingModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ServoingModeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) +{ + + + if (RestoreFactorySettingsHandler) + { + res = RestoreFactorySettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/restore_factory_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) +{ + + m_is_activated_FactoryTopic = true; + + if (OnNotificationFactoryTopicHandler) + { + res = OnNotificationFactoryTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_factory_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) +{ + kortex_driver::FactoryNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_FactoryTopic.publish(ros_msg); +} + +bool BaseSimulationServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) +{ + + + if (GetAllConnectedControllersHandler) + { + res = GetAllConnectedControllersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_connected_controllers is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) +{ + + + if (GetControllerStateHandler) + { + res = GetControllerStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_controller_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) +{ + + + if (GetActuatorCountHandler) + { + res = GetActuatorCountHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_actuator_count is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) +{ + + + if (StartWifiScanHandler) + { + res = StartWifiScanHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/start_wifi_scan is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) +{ + + + if (GetConfiguredWifiHandler) + { + res = GetConfiguredWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_configured_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) +{ + + m_is_activated_NetworkTopic = true; + + if (OnNotificationNetworkTopicHandler) + { + res = OnNotificationNetworkTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_network_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) +{ + kortex_driver::NetworkNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_NetworkTopic.publish(ros_msg); +} + +bool BaseSimulationServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) +{ + + + if (GetArmStateHandler) + { + res = GetArmStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_arm_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) +{ + + m_is_activated_ArmStateTopic = true; + + if (OnNotificationArmStateTopicHandler) + { + res = OnNotificationArmStateTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_arm_state_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) +{ + kortex_driver::ArmStateNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ArmStateTopic.publish(ros_msg); +} + +bool BaseSimulationServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) +{ + + + if (GetIPv4InformationHandler) + { + res = GetIPv4InformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_i_pv4_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) +{ + + + if (SetWifiCountryCodeHandler) + { + res = SetWifiCountryCodeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_wifi_country_code is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) +{ + + + if (GetWifiCountryCodeHandler) + { + res = GetWifiCountryCodeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wifi_country_code is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) +{ + + + if (Base_SetCapSenseConfigHandler) + { + res = Base_SetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) +{ + + + if (Base_GetCapSenseConfigHandler) + { + res = Base_GetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_speed_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsSpeedHardLimitationHandler) + { + res = GetAllJointsSpeedHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_speed_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_torque_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsTorqueHardLimitationHandler) + { + res = GetAllJointsTorqueHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_torque_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) +{ + ROS_WARN("The base/get_twist_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetTwistHardLimitationHandler) + { + res = GetTwistHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_twist_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) +{ + ROS_WARN("The base/get_wrench_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetWrenchHardLimitationHandler) + { + res = GetWrenchHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wrench_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) +{ + + + if (SendJointSpeedsJoystickCommandHandler) + { + res = SendJointSpeedsJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_joint_speeds_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) +{ + + + if (SendSelectedJointSpeedJoystickCommandHandler) + { + res = SendSelectedJointSpeedJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_selected_joint_speed_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) +{ + + + if (EnableBridgeHandler) + { + res = EnableBridgeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/enable_bridge is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) +{ + + + if (DisableBridgeHandler) + { + res = DisableBridgeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/disable_bridge is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) +{ + + + if (GetBridgeListHandler) + { + res = GetBridgeListHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_bridge_list is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) +{ + + + if (GetBridgeConfigHandler) + { + res = GetBridgeConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_bridge_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) +{ + + + if (PlayPreComputedJointTrajectoryHandler) + { + res = PlayPreComputedJointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_pre_computed_joint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) +{ + + + if (GetProductConfigurationHandler) + { + res = GetProductConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_product_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) +{ + + + if (UpdateEndEffectorTypeConfigurationHandler) + { + res = UpdateEndEffectorTypeConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_end_effector_type_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) +{ + + + if (RestoreFactoryProductConfigurationHandler) + { + res = RestoreFactoryProductConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/restore_factory_product_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) +{ + + + if (GetTrajectoryErrorReportHandler) + { + res = GetTrajectoryErrorReportHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_trajectory_error_report is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_speed_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsSpeedSoftLimitationHandler) + { + res = GetAllJointsSpeedSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_speed_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_torque_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsTorqueSoftLimitationHandler) + { + res = GetAllJointsTorqueSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_torque_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_twist_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetTwistSoftLimitationHandler) + { + res = GetTwistSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_twist_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_wrench_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetWrenchSoftLimitationHandler) + { + res = GetWrenchSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wrench_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) +{ + + + if (SetControllerConfigurationModeHandler) + { + res = SetControllerConfigurationModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_controller_configuration_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) +{ + + + if (GetControllerConfigurationModeHandler) + { + res = GetControllerConfigurationModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_controller_configuration_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) +{ + + + if (StartTeachingHandler) + { + res = StartTeachingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/start_teaching is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) +{ + + + if (StopTeachingHandler) + { + res = StopTeachingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop_teaching is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) +{ + + + if (AddSequenceTasksHandler) + { + res = AddSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/add_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) +{ + + + if (UpdateSequenceTaskHandler) + { + res = UpdateSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) +{ + + + if (SwapSequenceTasksHandler) + { + res = SwapSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/swap_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) +{ + + + if (ReadSequenceTaskHandler) + { + res = ReadSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) +{ + + + if (ReadAllSequenceTasksHandler) + { + res = ReadAllSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) +{ + + + if (DeleteSequenceTaskHandler) + { + res = DeleteSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) +{ + + + if (DeleteAllSequenceTasksHandler) + { + res = DeleteAllSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_all_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) +{ + + + if (TakeSnapshotHandler) + { + res = TakeSnapshotHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/take_snapshot is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) +{ + + + if (GetFirmwareBundleVersionsHandler) + { + res = GetFirmwareBundleVersionsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_firmware_bundle_versions is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) +{ + + + if (MoveSequenceTaskHandler) + { + res = MoveSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/move_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) +{ + + + if (DuplicateMappingHandler) + { + res = DuplicateMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/duplicate_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) +{ + + + if (DuplicateMapHandler) + { + res = DuplicateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/duplicate_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) +{ + + + if (SetControllerConfigurationHandler) + { + res = SetControllerConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_controller_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) +{ + + + if (GetControllerConfigurationHandler) + { + res = GetControllerConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_controller_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) +{ + + + if (GetAllControllerConfigurationsHandler) + { + res = GetAllControllerConfigurationsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_controller_configurations is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/controlconfig_services.cpp b/kortex_driver/src/generated/simulation/controlconfig_services.cpp new file mode 100644 index 00000000..1dfd2669 --- /dev/null +++ b/kortex_driver/src/generated/simulation/controlconfig_services.cpp @@ -0,0 +1,539 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/controlconfig_services.h" + +ControlConfigSimulationServices::ControlConfigSimulationServices(ros::NodeHandle& node_handle): + IControlConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ControlConfigurationTopic = m_node_handle.advertise("control_configuration_topic", 1000); + m_is_activated_ControlConfigurationTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("control_config/set_device_id", &ControlConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("control_config/set_api_options", &ControlConfigSimulationServices::SetApiOptions, this); + + m_serviceSetGravityVector = m_node_handle.advertiseService("control_config/set_gravity_vector", &ControlConfigSimulationServices::SetGravityVector, this); + m_serviceGetGravityVector = m_node_handle.advertiseService("control_config/get_gravity_vector", &ControlConfigSimulationServices::GetGravityVector, this); + m_serviceSetPayloadInformation = m_node_handle.advertiseService("control_config/set_payload_information", &ControlConfigSimulationServices::SetPayloadInformation, this); + m_serviceGetPayloadInformation = m_node_handle.advertiseService("control_config/get_payload_information", &ControlConfigSimulationServices::GetPayloadInformation, this); + m_serviceSetToolConfiguration = m_node_handle.advertiseService("control_config/set_tool_configuration", &ControlConfigSimulationServices::SetToolConfiguration, this); + m_serviceGetToolConfiguration = m_node_handle.advertiseService("control_config/get_tool_configuration", &ControlConfigSimulationServices::GetToolConfiguration, this); + m_serviceOnNotificationControlConfigurationTopic = m_node_handle.advertiseService("control_config/activate_publishing_of_control_configuration_topic", &ControlConfigSimulationServices::OnNotificationControlConfigurationTopic, this); + m_serviceControlConfig_Unsubscribe = m_node_handle.advertiseService("control_config/unsubscribe", &ControlConfigSimulationServices::ControlConfig_Unsubscribe, this); + m_serviceSetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/set_cartesian_reference_frame", &ControlConfigSimulationServices::SetCartesianReferenceFrame, this); + m_serviceGetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/get_cartesian_reference_frame", &ControlConfigSimulationServices::GetCartesianReferenceFrame, this); + m_serviceControlConfig_GetControlMode = m_node_handle.advertiseService("control_config/get_control_mode", &ControlConfigSimulationServices::ControlConfig_GetControlMode, this); + m_serviceSetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/set_joint_speed_soft_limits", &ControlConfigSimulationServices::SetJointSpeedSoftLimits, this); + m_serviceSetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/set_twist_linear_soft_limit", &ControlConfigSimulationServices::SetTwistLinearSoftLimit, this); + m_serviceSetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/set_twist_angular_soft_limit", &ControlConfigSimulationServices::SetTwistAngularSoftLimit, this); + m_serviceSetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/set_joint_acceleration_soft_limits", &ControlConfigSimulationServices::SetJointAccelerationSoftLimits, this); + m_serviceGetKinematicHardLimits = m_node_handle.advertiseService("control_config/get_kinematic_hard_limits", &ControlConfigSimulationServices::GetKinematicHardLimits, this); + m_serviceGetKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_kinematic_soft_limits", &ControlConfigSimulationServices::GetKinematicSoftLimits, this); + m_serviceGetAllKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_all_kinematic_soft_limits", &ControlConfigSimulationServices::GetAllKinematicSoftLimits, this); + m_serviceSetDesiredLinearTwist = m_node_handle.advertiseService("control_config/set_desired_linear_twist", &ControlConfigSimulationServices::SetDesiredLinearTwist, this); + m_serviceSetDesiredAngularTwist = m_node_handle.advertiseService("control_config/set_desired_angular_twist", &ControlConfigSimulationServices::SetDesiredAngularTwist, this); + m_serviceSetDesiredJointSpeeds = m_node_handle.advertiseService("control_config/set_desired_joint_speeds", &ControlConfigSimulationServices::SetDesiredJointSpeeds, this); + m_serviceGetDesiredSpeeds = m_node_handle.advertiseService("control_config/get_desired_speeds", &ControlConfigSimulationServices::GetDesiredSpeeds, this); + m_serviceResetGravityVector = m_node_handle.advertiseService("control_config/reset_gravity_vector", &ControlConfigSimulationServices::ResetGravityVector, this); + m_serviceResetPayloadInformation = m_node_handle.advertiseService("control_config/reset_payload_information", &ControlConfigSimulationServices::ResetPayloadInformation, this); + m_serviceResetToolConfiguration = m_node_handle.advertiseService("control_config/reset_tool_configuration", &ControlConfigSimulationServices::ResetToolConfiguration, this); + m_serviceResetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_speed_soft_limits", &ControlConfigSimulationServices::ResetJointSpeedSoftLimits, this); + m_serviceResetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigSimulationServices::ResetTwistLinearSoftLimit, this); + m_serviceResetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigSimulationServices::ResetTwistAngularSoftLimit, this); + m_serviceResetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigSimulationServices::ResetJointAccelerationSoftLimits, this); +} + +bool ControlConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool ControlConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool ControlConfigSimulationServices::SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) +{ + + + if (SetGravityVectorHandler) + { + res = SetGravityVectorHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_gravity_vector is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) +{ + + + if (GetGravityVectorHandler) + { + res = GetGravityVectorHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_gravity_vector is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) +{ + + + if (SetPayloadInformationHandler) + { + res = SetPayloadInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_payload_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) +{ + + + if (GetPayloadInformationHandler) + { + res = GetPayloadInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_payload_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) +{ + + + if (SetToolConfigurationHandler) + { + res = SetToolConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_tool_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) +{ + + + if (GetToolConfigurationHandler) + { + res = GetToolConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_tool_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) +{ + + m_is_activated_ControlConfigurationTopic = true; + + if (OnNotificationControlConfigurationTopicHandler) + { + res = OnNotificationControlConfigurationTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/activate_publishing_of_control_configuration_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void ControlConfigSimulationServices::cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) +{ + kortex_driver::ControlConfigurationNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlConfigurationTopic.publish(ros_msg); +} + +bool ControlConfigSimulationServices::ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) +{ + + + if (ControlConfig_UnsubscribeHandler) + { + res = ControlConfig_UnsubscribeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/unsubscribe is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) +{ + + + if (SetCartesianReferenceFrameHandler) + { + res = SetCartesianReferenceFrameHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_cartesian_reference_frame is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) +{ + + + if (GetCartesianReferenceFrameHandler) + { + res = GetCartesianReferenceFrameHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_cartesian_reference_frame is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) +{ + + + if (ControlConfig_GetControlModeHandler) + { + res = ControlConfig_GetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) +{ + + + if (SetJointSpeedSoftLimitsHandler) + { + res = SetJointSpeedSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_joint_speed_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) +{ + + + if (SetTwistLinearSoftLimitHandler) + { + res = SetTwistLinearSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_twist_linear_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) +{ + + + if (SetTwistAngularSoftLimitHandler) + { + res = SetTwistAngularSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_twist_angular_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) +{ + + + if (SetJointAccelerationSoftLimitsHandler) + { + res = SetJointAccelerationSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_joint_acceleration_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) +{ + + + if (GetKinematicHardLimitsHandler) + { + res = GetKinematicHardLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_kinematic_hard_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) +{ + + + if (GetKinematicSoftLimitsHandler) + { + res = GetKinematicSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_kinematic_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) +{ + + + if (GetAllKinematicSoftLimitsHandler) + { + res = GetAllKinematicSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_all_kinematic_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) +{ + + + if (SetDesiredLinearTwistHandler) + { + res = SetDesiredLinearTwistHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_desired_linear_twist is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) +{ + + + if (SetDesiredAngularTwistHandler) + { + res = SetDesiredAngularTwistHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_desired_angular_twist is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) +{ + + + if (SetDesiredJointSpeedsHandler) + { + res = SetDesiredJointSpeedsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_desired_joint_speeds is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) +{ + + + if (GetDesiredSpeedsHandler) + { + res = GetDesiredSpeedsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_desired_speeds is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) +{ + + + if (ResetGravityVectorHandler) + { + res = ResetGravityVectorHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_gravity_vector is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) +{ + + + if (ResetPayloadInformationHandler) + { + res = ResetPayloadInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_payload_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) +{ + + + if (ResetToolConfigurationHandler) + { + res = ResetToolConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_tool_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) +{ + + + if (ResetJointSpeedSoftLimitsHandler) + { + res = ResetJointSpeedSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_joint_speed_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) +{ + + + if (ResetTwistLinearSoftLimitHandler) + { + res = ResetTwistLinearSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_twist_linear_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) +{ + + + if (ResetTwistAngularSoftLimitHandler) + { + res = ResetTwistAngularSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_twist_angular_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) +{ + + + if (ResetJointAccelerationSoftLimitsHandler) + { + res = ResetJointAccelerationSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_joint_acceleration_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/deviceconfig_services.cpp b/kortex_driver/src/generated/simulation/deviceconfig_services.cpp new file mode 100644 index 00000000..0f17821f --- /dev/null +++ b/kortex_driver/src/generated/simulation/deviceconfig_services.cpp @@ -0,0 +1,587 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/deviceconfig_services.h" + +DeviceConfigSimulationServices::DeviceConfigSimulationServices(ros::NodeHandle& node_handle): + IDeviceConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_SafetyTopic = m_node_handle.advertise("safety_topic", 1000); + m_is_activated_SafetyTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("device_config/set_device_id", &DeviceConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_config/set_api_options", &DeviceConfigSimulationServices::SetApiOptions, this); + + m_serviceGetRunMode = m_node_handle.advertiseService("device_config/get_run_mode", &DeviceConfigSimulationServices::GetRunMode, this); + m_serviceSetRunMode = m_node_handle.advertiseService("device_config/set_run_mode", &DeviceConfigSimulationServices::SetRunMode, this); + m_serviceGetDeviceType = m_node_handle.advertiseService("device_config/get_device_type", &DeviceConfigSimulationServices::GetDeviceType, this); + m_serviceGetFirmwareVersion = m_node_handle.advertiseService("device_config/get_firmware_version", &DeviceConfigSimulationServices::GetFirmwareVersion, this); + m_serviceGetBootloaderVersion = m_node_handle.advertiseService("device_config/get_bootloader_version", &DeviceConfigSimulationServices::GetBootloaderVersion, this); + m_serviceGetModelNumber = m_node_handle.advertiseService("device_config/get_model_number", &DeviceConfigSimulationServices::GetModelNumber, this); + m_serviceGetPartNumber = m_node_handle.advertiseService("device_config/get_part_number", &DeviceConfigSimulationServices::GetPartNumber, this); + m_serviceGetSerialNumber = m_node_handle.advertiseService("device_config/get_serial_number", &DeviceConfigSimulationServices::GetSerialNumber, this); + m_serviceGetMACAddress = m_node_handle.advertiseService("device_config/get_m_a_c_address", &DeviceConfigSimulationServices::GetMACAddress, this); + m_serviceGetIPv4Settings = m_node_handle.advertiseService("device_config/get_i_pv4_settings", &DeviceConfigSimulationServices::GetIPv4Settings, this); + m_serviceSetIPv4Settings = m_node_handle.advertiseService("device_config/set_i_pv4_settings", &DeviceConfigSimulationServices::SetIPv4Settings, this); + m_serviceGetPartNumberRevision = m_node_handle.advertiseService("device_config/get_part_number_revision", &DeviceConfigSimulationServices::GetPartNumberRevision, this); + m_serviceRebootRequest = m_node_handle.advertiseService("device_config/reboot_request", &DeviceConfigSimulationServices::RebootRequest, this); + m_serviceSetSafetyEnable = m_node_handle.advertiseService("device_config/set_safety_enable", &DeviceConfigSimulationServices::SetSafetyEnable, this); + m_serviceSetSafetyErrorThreshold = m_node_handle.advertiseService("device_config/set_safety_error_threshold", &DeviceConfigSimulationServices::SetSafetyErrorThreshold, this); + m_serviceSetSafetyWarningThreshold = m_node_handle.advertiseService("device_config/set_safety_warning_threshold", &DeviceConfigSimulationServices::SetSafetyWarningThreshold, this); + m_serviceSetSafetyConfiguration = m_node_handle.advertiseService("device_config/set_safety_configuration", &DeviceConfigSimulationServices::SetSafetyConfiguration, this); + m_serviceGetSafetyConfiguration = m_node_handle.advertiseService("device_config/get_safety_configuration", &DeviceConfigSimulationServices::GetSafetyConfiguration, this); + m_serviceGetSafetyInformation = m_node_handle.advertiseService("device_config/get_safety_information", &DeviceConfigSimulationServices::GetSafetyInformation, this); + m_serviceGetSafetyEnable = m_node_handle.advertiseService("device_config/get_safety_enable", &DeviceConfigSimulationServices::GetSafetyEnable, this); + m_serviceGetSafetyStatus = m_node_handle.advertiseService("device_config/get_safety_status", &DeviceConfigSimulationServices::GetSafetyStatus, this); + m_serviceClearAllSafetyStatus = m_node_handle.advertiseService("device_config/clear_all_safety_status", &DeviceConfigSimulationServices::ClearAllSafetyStatus, this); + m_serviceClearSafetyStatus = m_node_handle.advertiseService("device_config/clear_safety_status", &DeviceConfigSimulationServices::ClearSafetyStatus, this); + m_serviceGetAllSafetyConfiguration = m_node_handle.advertiseService("device_config/get_all_safety_configuration", &DeviceConfigSimulationServices::GetAllSafetyConfiguration, this); + m_serviceGetAllSafetyInformation = m_node_handle.advertiseService("device_config/get_all_safety_information", &DeviceConfigSimulationServices::GetAllSafetyInformation, this); + m_serviceResetSafetyDefaults = m_node_handle.advertiseService("device_config/reset_safety_defaults", &DeviceConfigSimulationServices::ResetSafetyDefaults, this); + m_serviceOnNotificationSafetyTopic = m_node_handle.advertiseService("device_config/activate_publishing_of_safety_topic", &DeviceConfigSimulationServices::OnNotificationSafetyTopic, this); + m_serviceExecuteCalibration = m_node_handle.advertiseService("device_config/execute_calibration", &DeviceConfigSimulationServices::ExecuteCalibration, this); + m_serviceGetCalibrationResult = m_node_handle.advertiseService("device_config/get_calibration_result", &DeviceConfigSimulationServices::GetCalibrationResult, this); + m_serviceStopCalibration = m_node_handle.advertiseService("device_config/stop_calibration", &DeviceConfigSimulationServices::StopCalibration, this); + m_serviceDeviceConfig_SetCapSenseConfig = m_node_handle.advertiseService("device_config/set_cap_sense_config", &DeviceConfigSimulationServices::DeviceConfig_SetCapSenseConfig, this); + m_serviceDeviceConfig_GetCapSenseConfig = m_node_handle.advertiseService("device_config/get_cap_sense_config", &DeviceConfigSimulationServices::DeviceConfig_GetCapSenseConfig, this); +} + +bool DeviceConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool DeviceConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool DeviceConfigSimulationServices::GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) +{ + + + if (GetRunModeHandler) + { + res = GetRunModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_run_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) +{ + + + if (SetRunModeHandler) + { + res = SetRunModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_run_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) +{ + + + if (GetDeviceTypeHandler) + { + res = GetDeviceTypeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_device_type is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) +{ + + + if (GetFirmwareVersionHandler) + { + res = GetFirmwareVersionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_firmware_version is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) +{ + + + if (GetBootloaderVersionHandler) + { + res = GetBootloaderVersionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_bootloader_version is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) +{ + + + if (GetModelNumberHandler) + { + res = GetModelNumberHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_model_number is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) +{ + + + if (GetPartNumberHandler) + { + res = GetPartNumberHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_part_number is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) +{ + + + if (GetSerialNumberHandler) + { + res = GetSerialNumberHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_serial_number is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) +{ + + + if (GetMACAddressHandler) + { + res = GetMACAddressHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_m_a_c_address is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) +{ + + + if (GetIPv4SettingsHandler) + { + res = GetIPv4SettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_i_pv4_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) +{ + + + if (SetIPv4SettingsHandler) + { + res = SetIPv4SettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_i_pv4_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) +{ + + + if (GetPartNumberRevisionHandler) + { + res = GetPartNumberRevisionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_part_number_revision is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) +{ + + + if (RebootRequestHandler) + { + res = RebootRequestHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/reboot_request is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) +{ + + + if (SetSafetyEnableHandler) + { + res = SetSafetyEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) +{ + + + if (SetSafetyErrorThresholdHandler) + { + res = SetSafetyErrorThresholdHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_error_threshold is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) +{ + + + if (SetSafetyWarningThresholdHandler) + { + res = SetSafetyWarningThresholdHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_warning_threshold is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) +{ + + + if (SetSafetyConfigurationHandler) + { + res = SetSafetyConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) +{ + + + if (GetSafetyConfigurationHandler) + { + res = GetSafetyConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) +{ + + + if (GetSafetyInformationHandler) + { + res = GetSafetyInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) +{ + + + if (GetSafetyEnableHandler) + { + res = GetSafetyEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) +{ + + + if (GetSafetyStatusHandler) + { + res = GetSafetyStatusHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_status is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) +{ + + + if (ClearAllSafetyStatusHandler) + { + res = ClearAllSafetyStatusHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/clear_all_safety_status is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) +{ + + + if (ClearSafetyStatusHandler) + { + res = ClearSafetyStatusHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/clear_safety_status is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) +{ + + + if (GetAllSafetyConfigurationHandler) + { + res = GetAllSafetyConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_all_safety_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) +{ + + + if (GetAllSafetyInformationHandler) + { + res = GetAllSafetyInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_all_safety_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) +{ + + + if (ResetSafetyDefaultsHandler) + { + res = ResetSafetyDefaultsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/reset_safety_defaults is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) +{ + + m_is_activated_SafetyTopic = true; + + if (OnNotificationSafetyTopicHandler) + { + res = OnNotificationSafetyTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/activate_publishing_of_safety_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void DeviceConfigSimulationServices::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) +{ + kortex_driver::SafetyNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_SafetyTopic.publish(ros_msg); +} + +bool DeviceConfigSimulationServices::ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) +{ + + + if (ExecuteCalibrationHandler) + { + res = ExecuteCalibrationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/execute_calibration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) +{ + + + if (GetCalibrationResultHandler) + { + res = GetCalibrationResultHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_calibration_result is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) +{ + + + if (StopCalibrationHandler) + { + res = StopCalibrationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/stop_calibration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) +{ + + + if (DeviceConfig_SetCapSenseConfigHandler) + { + res = DeviceConfig_SetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) +{ + + + if (DeviceConfig_GetCapSenseConfigHandler) + { + res = DeviceConfig_GetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/devicemanager_services.cpp b/kortex_driver/src/generated/simulation/devicemanager_services.cpp new file mode 100644 index 00000000..f0be4c07 --- /dev/null +++ b/kortex_driver/src/generated/simulation/devicemanager_services.cpp @@ -0,0 +1,82 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/devicemanager_services.h" + +DeviceManagerSimulationServices::DeviceManagerSimulationServices(ros::NodeHandle& node_handle): + IDeviceManagerServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("device_manager/set_device_id", &DeviceManagerSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_manager/set_api_options", &DeviceManagerSimulationServices::SetApiOptions, this); + + m_serviceReadAllDevices = m_node_handle.advertiseService("device_manager/read_all_devices", &DeviceManagerSimulationServices::ReadAllDevices, this); +} + +bool DeviceManagerSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool DeviceManagerSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool DeviceManagerSimulationServices::ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) +{ + + + if (ReadAllDevicesHandler) + { + res = ReadAllDevicesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_manager/read_all_devices is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/interconnectconfig_services.cpp b/kortex_driver/src/generated/simulation/interconnectconfig_services.cpp new file mode 100644 index 00000000..32287a2b --- /dev/null +++ b/kortex_driver/src/generated/simulation/interconnectconfig_services.cpp @@ -0,0 +1,290 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/interconnectconfig_services.h" + +InterconnectConfigSimulationServices::InterconnectConfigSimulationServices(ros::NodeHandle& node_handle): + IInterconnectConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("interconnect_config/set_device_id", &InterconnectConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("interconnect_config/set_api_options", &InterconnectConfigSimulationServices::SetApiOptions, this); + + m_serviceGetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/get_u_a_r_t_configuration", &InterconnectConfigSimulationServices::GetUARTConfiguration, this); + m_serviceSetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/set_u_a_r_t_configuration", &InterconnectConfigSimulationServices::SetUARTConfiguration, this); + m_serviceGetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/get_ethernet_configuration", &InterconnectConfigSimulationServices::GetEthernetConfiguration, this); + m_serviceSetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/set_ethernet_configuration", &InterconnectConfigSimulationServices::SetEthernetConfiguration, this); + m_serviceGetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_configuration", &InterconnectConfigSimulationServices::GetGPIOConfiguration, this); + m_serviceSetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_configuration", &InterconnectConfigSimulationServices::SetGPIOConfiguration, this); + m_serviceGetGPIOState = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_state", &InterconnectConfigSimulationServices::GetGPIOState, this); + m_serviceSetGPIOState = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_state", &InterconnectConfigSimulationServices::SetGPIOState, this); + m_serviceGetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/get_i2_c_configuration", &InterconnectConfigSimulationServices::GetI2CConfiguration, this); + m_serviceSetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/set_i2_c_configuration", &InterconnectConfigSimulationServices::SetI2CConfiguration, this); + m_serviceI2CRead = m_node_handle.advertiseService("interconnect_config/i2_c_read", &InterconnectConfigSimulationServices::I2CRead, this); + m_serviceI2CReadRegister = m_node_handle.advertiseService("interconnect_config/i2_c_read_register", &InterconnectConfigSimulationServices::I2CReadRegister, this); + m_serviceI2CWrite = m_node_handle.advertiseService("interconnect_config/i2_c_write", &InterconnectConfigSimulationServices::I2CWrite, this); + m_serviceI2CWriteRegister = m_node_handle.advertiseService("interconnect_config/i2_c_write_register", &InterconnectConfigSimulationServices::I2CWriteRegister, this); +} + +bool InterconnectConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool InterconnectConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool InterconnectConfigSimulationServices::GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) +{ + + + if (GetUARTConfigurationHandler) + { + res = GetUARTConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_u_a_r_t_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) +{ + + + if (SetUARTConfigurationHandler) + { + res = SetUARTConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_u_a_r_t_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) +{ + + + if (GetEthernetConfigurationHandler) + { + res = GetEthernetConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_ethernet_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) +{ + + + if (SetEthernetConfigurationHandler) + { + res = SetEthernetConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_ethernet_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) +{ + + + if (GetGPIOConfigurationHandler) + { + res = GetGPIOConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_g_p_i_o_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) +{ + + + if (SetGPIOConfigurationHandler) + { + res = SetGPIOConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_g_p_i_o_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) +{ + + + if (GetGPIOStateHandler) + { + res = GetGPIOStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_g_p_i_o_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) +{ + + + if (SetGPIOStateHandler) + { + res = SetGPIOStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_g_p_i_o_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) +{ + + + if (GetI2CConfigurationHandler) + { + res = GetI2CConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_i2_c_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) +{ + + + if (SetI2CConfigurationHandler) + { + res = SetI2CConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_i2_c_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) +{ + + + if (I2CReadHandler) + { + res = I2CReadHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_read is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) +{ + + + if (I2CReadRegisterHandler) + { + res = I2CReadRegisterHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_read_register is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) +{ + + + if (I2CWriteHandler) + { + res = I2CWriteHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_write is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) +{ + + + if (I2CWriteRegisterHandler) + { + res = I2CWriteRegisterHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_write_register is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/visionconfig_services.cpp b/kortex_driver/src/generated/simulation/visionconfig_services.cpp new file mode 100644 index 00000000..b50c05c5 --- /dev/null +++ b/kortex_driver/src/generated/simulation/visionconfig_services.cpp @@ -0,0 +1,267 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/visionconfig_services.h" + +VisionConfigSimulationServices::VisionConfigSimulationServices(ros::NodeHandle& node_handle): + IVisionConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_VisionTopic = m_node_handle.advertise("vision_topic", 1000); + m_is_activated_VisionTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("vision_config/set_device_id", &VisionConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("vision_config/set_api_options", &VisionConfigSimulationServices::SetApiOptions, this); + + m_serviceSetSensorSettings = m_node_handle.advertiseService("vision_config/set_sensor_settings", &VisionConfigSimulationServices::SetSensorSettings, this); + m_serviceGetSensorSettings = m_node_handle.advertiseService("vision_config/get_sensor_settings", &VisionConfigSimulationServices::GetSensorSettings, this); + m_serviceGetOptionValue = m_node_handle.advertiseService("vision_config/get_option_value", &VisionConfigSimulationServices::GetOptionValue, this); + m_serviceSetOptionValue = m_node_handle.advertiseService("vision_config/set_option_value", &VisionConfigSimulationServices::SetOptionValue, this); + m_serviceGetOptionInformation = m_node_handle.advertiseService("vision_config/get_option_information", &VisionConfigSimulationServices::GetOptionInformation, this); + m_serviceOnNotificationVisionTopic = m_node_handle.advertiseService("vision_config/activate_publishing_of_vision_topic", &VisionConfigSimulationServices::OnNotificationVisionTopic, this); + m_serviceDoSensorFocusAction = m_node_handle.advertiseService("vision_config/do_sensor_focus_action", &VisionConfigSimulationServices::DoSensorFocusAction, this); + m_serviceGetIntrinsicParameters = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters", &VisionConfigSimulationServices::GetIntrinsicParameters, this); + m_serviceGetIntrinsicParametersProfile = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters_profile", &VisionConfigSimulationServices::GetIntrinsicParametersProfile, this); + m_serviceSetIntrinsicParameters = m_node_handle.advertiseService("vision_config/set_intrinsic_parameters", &VisionConfigSimulationServices::SetIntrinsicParameters, this); + m_serviceGetExtrinsicParameters = m_node_handle.advertiseService("vision_config/get_extrinsic_parameters", &VisionConfigSimulationServices::GetExtrinsicParameters, this); + m_serviceSetExtrinsicParameters = m_node_handle.advertiseService("vision_config/set_extrinsic_parameters", &VisionConfigSimulationServices::SetExtrinsicParameters, this); +} + +bool VisionConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool VisionConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool VisionConfigSimulationServices::SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) +{ + + + if (SetSensorSettingsHandler) + { + res = SetSensorSettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_sensor_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) +{ + + + if (GetSensorSettingsHandler) + { + res = GetSensorSettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_sensor_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) +{ + + + if (GetOptionValueHandler) + { + res = GetOptionValueHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_option_value is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) +{ + + + if (SetOptionValueHandler) + { + res = SetOptionValueHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_option_value is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) +{ + + + if (GetOptionInformationHandler) + { + res = GetOptionInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_option_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) +{ + + m_is_activated_VisionTopic = true; + + if (OnNotificationVisionTopicHandler) + { + res = OnNotificationVisionTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/activate_publishing_of_vision_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void VisionConfigSimulationServices::cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) +{ + kortex_driver::VisionNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_VisionTopic.publish(ros_msg); +} + +bool VisionConfigSimulationServices::DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) +{ + + + if (DoSensorFocusActionHandler) + { + res = DoSensorFocusActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/do_sensor_focus_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) +{ + + + if (GetIntrinsicParametersHandler) + { + res = GetIntrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_intrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) +{ + + + if (GetIntrinsicParametersProfileHandler) + { + res = GetIntrinsicParametersProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_intrinsic_parameters_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) +{ + + + if (SetIntrinsicParametersHandler) + { + res = SetIntrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_intrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) +{ + + + if (GetExtrinsicParametersHandler) + { + res = GetExtrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_extrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) +{ + + + if (SetExtrinsicParametersHandler) + { + res = SetExtrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_extrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index a93a4d20..3a8f3697 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -12,23 +12,48 @@ #include "kortex_driver/non-generated/kortex_arm_driver.h" -KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), m_node_is_running(true), m_consecutive_base_cyclic_timeouts(0) +KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), + m_node_is_running(true), + m_consecutive_base_cyclic_timeouts(0), + m_is_interconnect_module_present(false), + m_is_vision_module_present(false), + m_simulator{} { // Parameter to let the other nodes know this node is up ros::param::set("is_initialized", false); - // Start the node in the different initialization functions parseRosArguments(); - initApi(); - verifyProductConfiguration(); - initSubscribers(); + + // If with a real robot, start the robot-specific stuff + if (m_is_real_robot) + { + initApi(); + verifyProductConfiguration(); + initSubscribers(); + startActionServers(); + } + + // ROS Services are always started initRosServices(); - startActionServers(); + + // Enable ROS Service simulation if not with a real robot + if (!m_is_real_robot) + { + m_simulator.reset(new KortexArmSimulation(nh)); + registerSimulationHandlers(); + } // Start the thread to publish the feedback and joint states m_pub_base_feedback = m_node_handle.advertise("base_feedback", 1000); m_pub_joint_state = m_node_handle.advertise("base_feedback/joint_state", 1000); - m_publish_feedback_thread = std::thread(&KortexArmDriver::publishFeedback, this); + if (m_is_real_robot) + { + m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this); + } + else + { + m_publish_feedback_thread = std::thread(&KortexArmDriver::publishSimulationFeedback, this); + } // If we get here and no error was thrown we started the node correctly ROS_INFO("%sThe Kortex driver has been initialized correctly!%s", GREEN_COLOR_CONSOLE, RESET_COLOR_CONSOLE); @@ -44,14 +69,6 @@ KortexArmDriver::~KortexArmDriver() m_publish_feedback_thread.join(); } - delete m_action_server_follow_joint_trajectory; - if (m_action_server_gripper_command) - { - delete m_action_server_gripper_command; - } - - delete m_topic_subscribers; - delete m_actuator_config_ros_services; delete m_base_ros_services; delete m_control_config_ros_services; @@ -65,100 +82,126 @@ KortexArmDriver::~KortexArmDriver() { delete m_vision_config_ros_services; } - - m_tcp_session_manager->CloseSession(); - m_udp_session_manager->CloseSession(); - m_tcp_router->SetActivationStatus(false); - m_udp_router->SetActivationStatus(false); - m_tcp_transport->disconnect(); - m_udp_transport->disconnect(); - - delete m_actuator_config; - delete m_base; - delete m_control_config; - delete m_device_config; - delete m_device_manager; - delete m_interconnect_config; - delete m_vision_config; - delete m_base_cyclic; - - delete m_tcp_session_manager; - delete m_udp_session_manager; - - delete m_tcp_router; - delete m_udp_router; - delete m_tcp_transport; - delete m_udp_transport; -} -void KortexArmDriver::parseRosArguments() -{ - bool use_sim_time = false; - if (ros::param::get("/use_sim_time", use_sim_time)) + if (!m_is_real_robot) { - if (use_sim_time) + + delete m_action_server_follow_joint_trajectory; + if (m_action_server_gripper_command) { - std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver."; - ROS_WARN("%s", error_string.c_str()); + delete m_action_server_gripper_command; } - } - if (!ros::param::get("~ip_address", m_ip_address)) - { - std::string error_string = "IP address of the robot was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + delete m_topic_subscribers; - if (!ros::param::get("~username", m_username)) - { - std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); + m_tcp_session_manager->CloseSession(); + m_udp_session_manager->CloseSession(); + m_tcp_router->SetActivationStatus(false); + m_udp_router->SetActivationStatus(false); + m_tcp_transport->disconnect(); + m_udp_transport->disconnect(); + + delete m_actuator_config; + delete m_base; + delete m_control_config; + delete m_device_config; + delete m_device_manager; + delete m_interconnect_config; + delete m_vision_config; + delete m_base_cyclic; + + delete m_tcp_session_manager; + delete m_udp_session_manager; + + delete m_tcp_router; + delete m_udp_router; + delete m_tcp_transport; + delete m_udp_transport; } +} - if (!ros::param::get("~password", m_password)) +void KortexArmDriver::parseRosArguments() +{ + bool sim; + if (!ros::param::get("~sim", sim)) { - std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node..."; + std::string error_string = "Simulation was not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + m_is_real_robot = !sim; - if (!ros::param::get("~use_hard_limits", m_use_hard_limits)) + // Some parameters are only necessary with a real robot + if (m_is_real_robot) { - std::string error_string = "Usage of hard limits as soft was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + bool use_sim_time = false; + if (ros::param::get("/use_sim_time", use_sim_time)) + { + if (use_sim_time) + { + std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver."; + ROS_WARN("%s", error_string.c_str()); + } + } - if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate)) - { - std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + if (!ros::param::get("~ip_address", m_ip_address)) + { + std::string error_string = "IP address of the robot was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } - if (!ros::param::get("~api_rpc_timeout_ms", m_api_rpc_timeout_ms)) - { - std::string error_string = "API RPC timeout duration was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + if (!ros::param::get("~username", m_username)) + { + std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } - if (!ros::param::get("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms)) - { - std::string error_string = "API session inactivity timeout duration was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); + if (!ros::param::get("~password", m_password)) + { + std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~use_hard_limits", m_use_hard_limits)) + { + std::string error_string = "Usage of hard limits as soft was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_rpc_timeout_ms", m_api_rpc_timeout_ms)) + { + std::string error_string = "API RPC timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms)) + { + std::string error_string = "API session inactivity timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms)) + { + std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } } - if (!ros::param::get("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms)) + + if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate)) { - std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node..."; + std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } - + if (!ros::param::get("~arm", m_arm_name)) { std::string error_string = "Arm name was not specified in the launch file, shutting down the node..."; @@ -186,6 +229,13 @@ void KortexArmDriver::parseRosArguments() ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + std::string robot_name; + if (!ros::param::get("~robot_name", robot_name)) + { + std::string error_string = "Robot name was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } if (!ros::param::get("~prefix", m_prefix)) { std::string error_string = "Prefix name was not specified in the launch file, shutting down the node..."; @@ -364,11 +414,7 @@ void KortexArmDriver::verifyProductConfiguration() ROS_INFO("-------------------------------------------------"); ROS_INFO("Scanning all devices in robot... "); - // We suppose we don't have an interconnect or a vision module until we find it - m_is_interconnect_module_present = false; - m_is_vision_module_present = false; - - // Loop through the devices to find the device ID's +// Loop through the devices to find the device ID's auto devices = m_device_manager->ReadAllDevices(); for (auto device : devices.device_handle()) { @@ -415,26 +461,53 @@ void KortexArmDriver::initRosServices() { ROS_INFO("-------------------------------------------------"); ROS_INFO("Initializing Kortex Driver's services..."); - m_actuator_config_ros_services = new ActuatorConfigServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); - m_base_ros_services = new BaseServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); - m_control_config_ros_services = new ControlConfigServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); - m_device_config_ros_services = new DeviceConfigServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); - m_device_manager_ros_services = new DeviceManagerServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); - if (m_is_interconnect_module_present) - { - m_interconnect_config_ros_services = new InterconnectConfigServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); - } - else - { - m_interconnect_config_ros_services = nullptr; - } - if (m_is_vision_module_present) - { - m_vision_config_ros_services = new VisionConfigServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + if (m_is_real_robot) + { + m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); + m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); + m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); + m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); + m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); + } + else + { + m_interconnect_config_ros_services = nullptr; + } + if (m_is_vision_module_present) + { + m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + } + else + { + m_vision_config_ros_services = nullptr; + } } else { - m_vision_config_ros_services = nullptr; + m_actuator_config_ros_services = new ActuatorConfigSimulationServices(m_node_handle); + m_base_ros_services = new BaseSimulationServices(m_node_handle); + m_control_config_ros_services = new ControlConfigSimulationServices(m_node_handle); + m_device_config_ros_services = new DeviceConfigSimulationServices(m_node_handle); + m_device_manager_ros_services = new DeviceManagerSimulationServices(m_node_handle); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigSimulationServices(m_node_handle); + } + else + { + m_interconnect_config_ros_services = nullptr; + } + if (m_is_vision_module_present) + { + m_vision_config_ros_services = new VisionConfigSimulationServices(m_node_handle); + } + else + { + m_vision_config_ros_services = nullptr; + } } std::this_thread::sleep_for(std::chrono::milliseconds(2000)); ROS_INFO("Kortex Driver's services initialized correctly."); @@ -492,7 +565,7 @@ void KortexArmDriver::setAngularTrajectorySoftLimitsToMax() } } -void KortexArmDriver::publishFeedback() +void KortexArmDriver::publishRobotFeedback() { Kinova::Api::BaseCyclic::Feedback feedback_from_api; @@ -569,3 +642,44 @@ void KortexArmDriver::publishFeedback() rate.sleep(); } } + +void KortexArmDriver::publishSimulationFeedback() +{ + ros::Rate rate(m_cyclic_data_publish_rate); + while (m_node_is_running) + { + m_pub_base_feedback.publish(m_simulator->GetFeedback()); + rate.sleep(); + } +} + +void KortexArmDriver::registerSimulationHandlers() +{ + BaseSimulationServices* base_services_simulation = dynamic_cast(m_base_ros_services); + // Link the m_simulator handlers to the ROS services callbacks + // Action services + base_services_simulation->CreateActionHandler = std::bind(&KortexArmSimulation::CreateAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ReadActionHandler = std::bind(&KortexArmSimulation::ReadAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ReadAllActionsHandler = std::bind(&KortexArmSimulation::ReadAllActions, m_simulator.get(), std::placeholders::_1); + base_services_simulation->DeleteActionHandler = std::bind(&KortexArmSimulation::DeleteAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->UpdateActionHandler = std::bind(&KortexArmSimulation::UpdateAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ExecuteActionFromReferenceHandler = std::bind(&KortexArmSimulation::ExecuteActionFromReference, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ExecuteActionHandler = std::bind(&KortexArmSimulation::ExecuteAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1); + // Other services + base_services_simulation->PlayCartesianTrajectoryHandler = std::bind(&KortexArmSimulation::PlayCartesianTrajectory, m_simulator.get(), std::placeholders::_1); + base_services_simulation->PlayJointTrajectoryHandler = std::bind(&KortexArmSimulation::PlayJointTrajectory, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendJointSpeedsCommandHandler = std::bind(&KortexArmSimulation::SendJointSpeedsCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendGripperCommandHandler = std::bind(&KortexArmSimulation::SendGripperCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopHandler = std::bind(&KortexArmSimulation::Stop, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ApplyEmergencyStopHandler = std::bind(&KortexArmSimulation::ApplyEmergencyStop, m_simulator.get(), std::placeholders::_1); + + // Uncomment this SendTwistCommand handler to use the twist command simulation - not stable + // base_services_simulation->SendTwistCommandHandler = std::bind(&KortexArmSimulation::SendTwistCommand, m_simulator.get(), std::placeholders::_1); + + // Prospects + //SendSelectedJointSpeedCommand + //PlaySelectedJointTrajectory + //PlayCartesianTrajectoryPosition + //PlayCartesianTrajectoryOrientation +} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp new file mode 100644 index 00000000..779fdca5 --- /dev/null +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -0,0 +1,1652 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2020 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +#include "kortex_driver/non-generated/kortex_arm_simulation.h" +#include "kortex_driver/ErrorCodes.h" +#include "kortex_driver/SubErrorCodes.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/ActionEvent.h" +#include "kortex_driver/JointTrajectoryConstraintType.h" +#include "kortex_driver/GripperMode.h" +#include "kortex_driver/CartesianReferenceFrame.h" + +#include "trajectory_msgs/JointTrajectory.h" +#include "controller_manager_msgs/SwitchController.h" +#include "std_msgs/Float64.h" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace +{ + static const std::string ARM_PLANNING_GROUP = "arm"; + static const std::string GRIPPER_PLANNING_GROUP = "gripper"; + static constexpr unsigned int FIRST_CREATED_ACTION_ID = 10000; + static const std::set DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + static constexpr double JOINT_TRAJECTORY_TIMESTEP_SECONDS = 0.01; + static constexpr double MINIMUM_JOINT_VELOCITY_RAD_PER_SECONDS = 0.001; +} + +KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle), + m_map_actions{}, + m_is_action_being_executed{false}, + m_action_preempted{false}, + m_current_action_type{0}, + m_first_state_received{false}, + m_active_controller_type{ControllerType::kTrajectory} +{ + // Namespacing and prefixing information + ros::param::get("~robot_name", m_robot_name); + ros::param::get("~prefix", m_prefix); + + // Arm information + urdf::Model model; + model.initParam("/" + m_robot_name + "/robot_description"); + ros::param::get("~dof", m_degrees_of_freedom); + ros::param::get("~arm", m_arm_name); + ros::param::get("~joint_names", m_arm_joint_names); + ros::param::get("~maximum_velocities", m_arm_velocity_max_limits); + ros::param::get("~maximum_accelerations", m_arm_acceleration_max_limits); + m_arm_joint_limits_min.resize(GetDOF()); + m_arm_joint_limits_max.resize(GetDOF()); + for (int i = 0; i < GetDOF(); i++) + { + auto joint = model.getJoint(m_arm_joint_names[i]); + m_arm_joint_limits_min[i] = joint->limits->lower; + m_arm_joint_limits_max[i] = joint->limits->upper; + } + + // Cartesian Twist limits + ros::param::get("~maximum_linear_velocity", m_max_cartesian_twist_linear); + ros::param::get("~maximum_angular_velocity", m_max_cartesian_twist_angular); + ros::param::get("~maximum_linear_acceleration", m_max_cartesian_acceleration_linear); + ros::param::get("~maximum_angular_acceleration", m_max_cartesian_acceleration_angular); + + // Gripper information + ros::param::get("~gripper", m_gripper_name); + if (IsGripperPresent()) + { + ros::param::get("~gripper_joint_names", m_gripper_joint_names); + + // The joint_states feedback uses alphabetical order for the indexes + // If the gripper joint is before + if (m_gripper_joint_names[0] < m_arm_joint_names[0]) + { + m_gripper_joint_index = 0; + m_first_arm_joint_index = 1; + } + // If the gripper joint is after the arm joints + else + { + m_gripper_joint_index = GetDOF(); + m_first_arm_joint_index = 0; + } + + for (auto s : m_gripper_joint_names) + { + s.insert(0, m_prefix); + } + ros::param::get("~gripper_joint_limits_max", m_gripper_joint_limits_max); + ros::param::get("~gripper_joint_limits_min", m_gripper_joint_limits_min); + } + + // Print out simulation configuration + ROS_INFO("Simulating arm with following characteristics:"); + ROS_INFO("Arm type : %s", m_arm_name.c_str()); + ROS_INFO("Gripper type : %s", m_gripper_name.empty() ? "None" : m_gripper_name.c_str()); + ROS_INFO("Arm namespace : %s", m_robot_name.c_str()); + ROS_INFO("URDF prefix : %s", m_prefix.c_str()); + + // Building the KDL chain from the robot description + // The chain goes from 'base_link' to 'tool_frame' + KDL::Tree tree; + if (!kdl_parser::treeFromParam("robot_description", tree)) + { + const std::string error_string("Failed to parse robot_description parameter to build the kinematic tree!"); + ROS_ERROR("%s", error_string.c_str()); + throw(std::runtime_error(error_string)); + } + if (!tree.getChain(m_prefix + "base_link", m_prefix + "tool_frame", m_chain)) + { + const std::string error_string("Failed to extract kinematic chain from parsed tree!"); + ROS_ERROR("%s", error_string.c_str()); + throw(std::runtime_error(error_string)); + } + m_fk_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain)); + m_ik_vel_solver.reset(new KDL::ChainIkSolverVel_pinv(m_chain)); + m_ik_pos_solver.reset(new KDL::ChainIkSolverPos_NR(m_chain, *m_fk_solver, *m_ik_vel_solver)); + + // Build the velocity profile for each joint using the max velocities and max accelerations + for (int i = 0; i < GetDOF(); i++) + { + m_velocity_trap_profiles.push_back(KDL::VelocityProfile_Trap(m_arm_velocity_max_limits[i], m_arm_acceleration_max_limits[i])); + } + + // Start MoveIt client + m_moveit_arm_interface.reset(new moveit::planning_interface::MoveGroupInterface(ARM_PLANNING_GROUP)); + if (IsGripperPresent()) + { + m_moveit_gripper_interface.reset(new moveit::planning_interface::MoveGroupInterface(GRIPPER_PLANNING_GROUP)); + } + + // Create default actions + CreateDefaultActions(); + + // Create publishers and subscribers + for (int i = 0; i < GetDOF(); i++) + { + m_pub_position_controllers.push_back(m_node_handle.advertise( + "/" + m_robot_name + "/" + m_prefix + "joint_" + std::to_string(i+1) + "_position_controller/command", 1000)); + } + m_pub_action_topic = m_node_handle.advertise("action_topic", 1000); + m_sub_joint_state = m_node_handle.subscribe("/" + m_robot_name + "/" + "joint_states", 1, &KortexArmSimulation::cb_joint_states, this); + m_feedback.actuators.resize(GetDOF()); + m_feedback.interconnect.oneof_tool_feedback.gripper_feedback.resize(1); + m_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor.resize(1); + + m_sub_joint_speeds = m_node_handle.subscribe("in/joint_velocity", 1, &KortexArmSimulation::new_joint_speeds_cb, this); + m_sub_twist = m_node_handle.subscribe("in/cartesian_velocity", 1, &KortexArmSimulation::new_twist_cb, this); + m_sub_clear_faults = m_node_handle.subscribe("in/clear_faults", 1, &KortexArmSimulation::clear_faults_cb, this); + m_sub_stop = m_node_handle.subscribe("in/stop", 1, &KortexArmSimulation::stop_cb, this); + m_sub_emergency_stop = m_node_handle.subscribe("in/emergency_stop", 1, &KortexArmSimulation::emergency_stop_cb, this); + + // Create service clients + m_client_switch_controllers = m_node_handle.serviceClient + ("/" + m_robot_name + "/controller_manager/switch_controller"); + + // Fill controllers'names + m_trajectory_controllers_list.push_back(m_prefix + m_arm_name + "_joint_trajectory_controller"); // only one trajectory controller + m_position_controllers_list.resize(GetDOF()); // one position controller per + std::generate(m_position_controllers_list.begin(), + m_position_controllers_list.end(), + [this]() -> std::string + { + static int i = 1; + return m_prefix + "joint_" + std::to_string(i++) + "_position_controller"; + }); + + // Initialize the velocity commands to null + m_velocity_commands.resize(GetDOF()); + std::fill(m_velocity_commands.begin(), m_velocity_commands.end(), 0.0); + + // Create and connect action clients + m_follow_joint_trajectory_action_client.reset(new actionlib::SimpleActionClient( + "/" + m_robot_name + "/" + m_prefix + m_arm_name + "_joint_trajectory_controller" + "/follow_joint_trajectory", true)); + m_follow_joint_trajectory_action_client->waitForServer(); + if (IsGripperPresent()) + { + m_gripper_action_client.reset(new actionlib::SimpleActionClient( + "/" + m_robot_name + "/" + m_prefix + m_gripper_name + "_gripper_controller" + "/gripper_cmd", true)); + m_gripper_action_client->waitForServer(); + } + + // Create usual ROS parameters + m_node_handle.setParam("degrees_of_freedom", m_degrees_of_freedom); + m_node_handle.setParam("is_gripper_present", IsGripperPresent()); + m_node_handle.setParam("gripper_joint_names", m_gripper_joint_names); + m_node_handle.setParam("has_vision_module", false); + m_node_handle.setParam("has_interconnect_module", false); +} + +KortexArmSimulation::~KortexArmSimulation() +{ + JoinThreadAndCancelAction(); +} + +kortex_driver::BaseCyclic_Feedback KortexArmSimulation::GetFeedback() +{ + // If the feedback is not yet received, return now + if (!m_first_state_received) + { + return m_feedback; + } + + // Make a copy of current state + sensor_msgs::JointState current; + { + const std::lock_guard lock(m_state_mutex); + current = m_current_state; + } + + // Fill joint angles feedback + for (int i = 0; i < GetDOF(); i++) + { + m_feedback.actuators[i].position = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(current.position[m_first_arm_joint_index + i])); + m_feedback.actuators[i].velocity = m_math_util.toDeg(current.velocity[m_first_arm_joint_index + i]); + } + + // Calculate FK to get end effector pose + auto frame = KDL::Frame(); + Eigen::VectorXd positions_eigen(GetDOF()); + for (int i = 0; i < GetDOF(); i++) + { + positions_eigen[i] = current.position[m_first_arm_joint_index + i]; + } + KDL::JntArray current_kdl(GetDOF()); + current_kdl.data = positions_eigen; + m_fk_solver->JntToCart(current_kdl, frame); + m_feedback.base.tool_pose_x = frame.p.x(); + m_feedback.base.commanded_tool_pose_x = frame.p.x(); + m_feedback.base.tool_pose_y = frame.p.y(); + m_feedback.base.commanded_tool_pose_y = frame.p.y(); + m_feedback.base.tool_pose_z = frame.p.z(); + m_feedback.base.commanded_tool_pose_z = frame.p.z(); + double alpha, beta, gamma; + frame.M.GetEulerZYX(alpha, beta, gamma); + m_feedback.base.tool_pose_theta_x = m_math_util.toDeg(gamma); + m_feedback.base.commanded_tool_pose_theta_x = m_math_util.toDeg(gamma); + m_feedback.base.tool_pose_theta_y = m_math_util.toDeg(beta); + m_feedback.base.commanded_tool_pose_theta_y = m_math_util.toDeg(beta); + m_feedback.base.tool_pose_theta_z = m_math_util.toDeg(alpha); + m_feedback.base.commanded_tool_pose_theta_z = m_math_util.toDeg(alpha); + + // Fill gripper information + if (IsGripperPresent()) + { + // Gripper index is right after last actuator and is expressed in % in the base feedback (not in absolute position like in joint_states) + m_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor[0].position = 100.0 * m_math_util.relative_position_from_absolute(current.position[m_gripper_joint_index], m_gripper_joint_limits_min[0], m_gripper_joint_limits_max[0]); + } + + return m_feedback; +} + +kortex_driver::CreateAction::Response KortexArmSimulation::CreateAction(const kortex_driver::CreateAction::Request& req) +{ + auto new_action = req.input; + unsigned int identifier = FIRST_CREATED_ACTION_ID; + bool identifier_taken = true; + // Find unique identifier for new action + while (identifier_taken) + { + identifier_taken = m_map_actions.count(identifier) == 1; + if (identifier_taken) + { + ++identifier; + } + } + // Add Action to map if type is supported + switch (new_action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + case kortex_driver::ActionType::REACH_POSE: + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + case kortex_driver::ActionType::TIME_DELAY: + new_action.handle.identifier = identifier; + new_action.handle.permission = 7; + m_map_actions.emplace(std::make_pair(identifier, new_action)); + break; + default: + ROS_ERROR("Unsupported action type %d : could not create simulated action.", new_action.handle.action_type); + break; + } + // Return ActionHandle for added action + kortex_driver::CreateAction::Response response; + response.output = new_action.handle; + return response; +} + +kortex_driver::ReadAction::Response KortexArmSimulation::ReadAction(const kortex_driver::ReadAction::Request& req) +{ + auto input = req.input; + kortex_driver::ReadAction::Response response; + auto it = m_map_actions.find(input.identifier); + if (it != m_map_actions.end()) + { + response.output = it->second; + } + return response; +} + +kortex_driver::ReadAllActions::Response KortexArmSimulation::ReadAllActions(const kortex_driver::ReadAllActions::Request& req) +{ + auto input = req.input; + kortex_driver::ReadAllActions::Response response; + kortex_driver::ActionList action_list; + for (auto a : m_map_actions) + { + // If requested action type is specified and matches iterated action's type, add it to the list + if (input.action_type == 0 || input.action_type == a.second.handle.action_type) + { + action_list.action_list.push_back(a.second); + } + + } + response.output = action_list; + return response; +} + +kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const kortex_driver::DeleteAction::Request& req) +{ + auto handle = req.input; + // If the action is not a default action + if (DEFAULT_ACTIONS_IDENTIFIERS.find(handle.identifier) == DEFAULT_ACTIONS_IDENTIFIERS.end()) + { + auto it = m_map_actions.find(handle.identifier); + if (it != m_map_actions.end()) + { + m_map_actions.erase(it); + ROS_INFO("Simulated action #%u properly deleted.", handle.identifier); + } + else + { + ROS_WARN("Could not find simulated action #%u to delete in actions map.", handle.identifier); + } + } + else + { + ROS_ERROR("Cannot delete default simulated actions."); + } + + return kortex_driver::DeleteAction::Response(); +} + +kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const kortex_driver::UpdateAction::Request& req) +{ + auto action = req.input; + // If the action is not a default action + if (DEFAULT_ACTIONS_IDENTIFIERS.find(action.handle.identifier) == DEFAULT_ACTIONS_IDENTIFIERS.end()) + { + auto it = m_map_actions.find(action.handle.identifier); + if (it != m_map_actions.end()) + { + if (it->second.handle.action_type == action.handle.action_type) + { + it->second = action; + ROS_INFO("Simulated action #%u properly updated.", action.handle.identifier); + } + else + { + ROS_ERROR("Cannot update action with different type."); + } + } + else + { + ROS_ERROR("Could not find simulated action #%u to update in actions map.", action.handle.identifier); + } + } + else + { + ROS_ERROR("Cannot update default simulated actions."); + } + + return kortex_driver::UpdateAction::Response(); +} + +kortex_driver::ExecuteActionFromReference::Response KortexArmSimulation::ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req) +{ + auto handle = req.input; + auto it = m_map_actions.find(handle.identifier); + if (it != m_map_actions.end()) + { + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, it->second); + } + else + { + ROS_ERROR("Could not find action with given identifier %d", handle.identifier); + } + + return kortex_driver::ExecuteActionFromReference::Response(); +} + +kortex_driver::ExecuteAction::Response KortexArmSimulation::ExecuteAction(const kortex_driver::ExecuteAction::Request& req) +{ + auto action = req.input; + // Add Action to map if type is supported + switch (action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + case kortex_driver::ActionType::REACH_POSE: + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + case kortex_driver::ActionType::TIME_DELAY: + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + break; + default: + ROS_ERROR("Unsupported action type %d : could not execute simulated action.", action.handle.action_type); + break; + } + + return kortex_driver::ExecuteAction::Response(); +} + +kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex_driver::StopAction::Request& req) +{ + if (m_is_action_being_executed.load()) + { + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + } + + m_follow_joint_trajectory_action_client->cancelAllGoals(); + if (IsGripperPresent()) + { + m_gripper_action_client->cancelAllGoals(); + } + + return kortex_driver::StopAction::Response(); +} + +kortex_driver::PlayCartesianTrajectory::Response KortexArmSimulation::PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req) +{ + auto constrained_pose = req.input; + kortex_driver::Action action; + action.name = "PlayCartesianTrajectory"; + action.handle.action_type = kortex_driver::ActionType::REACH_POSE; + action.oneof_action_parameters.reach_pose.push_back(constrained_pose); + + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::PlayCartesianTrajectory::Response(); +} + +kortex_driver::SendTwistCommand::Response KortexArmSimulation::SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req) +{ + auto twist_command = req.input; + kortex_driver::Action action; + action.name = "SendTwistCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_TWIST_COMMAND; + action.oneof_action_parameters.send_twist_command.push_back(twist_command); + + // Convert orientations to rad + m_twist_command.angular_x = m_math_util.toRad(m_twist_command.angular_x); + m_twist_command.angular_x = m_math_util.toRad(m_twist_command.angular_y); + m_twist_command.angular_x = m_math_util.toRad(m_twist_command.angular_z); + + // Fill the twist command + m_twist_command = twist_command.twist; + + // If we are already executing twist control, don't cancel the thread + if (m_current_action_type != kortex_driver::ActionType::SEND_TWIST_COMMAND) + { + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + } + + return kortex_driver::SendTwistCommand::Response(); +} + +kortex_driver::PlayJointTrajectory::Response KortexArmSimulation::PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req) +{ + auto constrained_joint_angles = req.input; + kortex_driver::Action action; + action.name = "PlayJointTrajectory"; + action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + action.oneof_action_parameters.reach_joint_angles.push_back(constrained_joint_angles); + + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::PlayJointTrajectory::Response(); +} + +kortex_driver::SendJointSpeedsCommand::Response KortexArmSimulation::SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req) +{ + auto joint_speeds = req.input; + kortex_driver::Action action; + action.name = "SendJointSpeedsCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + action.oneof_action_parameters.send_joint_speeds.push_back(joint_speeds); + + // Fill the velocity commands vector + int n = 0; + std::generate(m_velocity_commands.begin(), + m_velocity_commands.end(), + [this, &action, &n]() -> double + { + return m_math_util.toRad(action.oneof_action_parameters.send_joint_speeds[0].joint_speeds[n++].value); + }); + + // If we are already executing joint speed control, don't cancel the thread + if (m_current_action_type != kortex_driver::ActionType::SEND_JOINT_SPEEDS) + { + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + } + + return kortex_driver::SendJointSpeedsCommand::Response(); +} + +kortex_driver::SendGripperCommand::Response KortexArmSimulation::SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req) +{ + auto gripper_command = req.input; + kortex_driver::Action action; + action.name = "GripperCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_GRIPPER_COMMAND; + action.oneof_action_parameters.send_gripper_command.push_back(gripper_command); + + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendGripperCommand::Response(); +} + +kortex_driver::Stop::Response KortexArmSimulation::Stop(const kortex_driver::Stop::Request& req) +{ + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + } + m_follow_joint_trajectory_action_client->cancelAllGoals(); + if (IsGripperPresent()) + { + m_gripper_action_client->cancelAllGoals(); + } + return kortex_driver::Stop::Response(); +} + +kortex_driver::ApplyEmergencyStop::Response KortexArmSimulation::ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req) +{ + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + JoinThreadAndCancelAction(); // this will block until the thread is joined and current action finished + } + m_follow_joint_trajectory_action_client->cancelAllGoals(); + if (IsGripperPresent()) + { + m_gripper_action_client->cancelAllGoals(); + } + return kortex_driver::ApplyEmergencyStop::Response(); +} + +void KortexArmSimulation::cb_joint_states(const sensor_msgs::JointState& state) +{ + const std::lock_guard lock(m_state_mutex); + m_first_state_received = true; + m_current_state = state; +} + +void KortexArmSimulation::CreateDefaultActions() +{ + kortex_driver::Action retract, home, zero; + kortex_driver::ConstrainedJointAngles retract_angles, home_angles, zero_angles; + // Retract + retract.handle.identifier = 1; + retract.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + retract.handle.permission = 7; + retract.name = "Retract"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("retract"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + retract_angles.joint_angles.joint_angles.push_back(a); + } + retract.oneof_action_parameters.reach_joint_angles.push_back(retract_angles); + // Home + home.handle.identifier = 2; + home.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + home.handle.permission = 7; + home.name = "Home"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("home"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + home_angles.joint_angles.joint_angles.push_back(a); + } + home.oneof_action_parameters.reach_joint_angles.push_back(home_angles); + // Zero + zero.handle.identifier = 3; + zero.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + zero.handle.permission = 7; + zero.name = "Zero"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("vertical"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + zero_angles.joint_angles.joint_angles.push_back(a); + } + zero.oneof_action_parameters.reach_joint_angles.push_back(zero_angles); + // Add actions + m_map_actions.emplace(std::make_pair(retract.handle.identifier, retract)); + m_map_actions.emplace(std::make_pair(home.handle.identifier, home)); + m_map_actions.emplace(std::make_pair(zero.handle.identifier, zero)); +} + +bool KortexArmSimulation::SwitchControllerType(ControllerType new_type) +{ + bool success = true; + controller_manager_msgs::SwitchController service; + service.request.strictness = service.request.STRICT; + if (m_active_controller_type != new_type) + { + // Set the controllers we want to switch to + switch (new_type) + { + case ControllerType::kTrajectory: + service.request.start_controllers = m_trajectory_controllers_list; + service.request.stop_controllers = m_position_controllers_list; + break; + case ControllerType::kIndividual: + service.request.start_controllers = m_position_controllers_list; + service.request.stop_controllers = m_trajectory_controllers_list; + break; + default: + ROS_ERROR("Kortex arm simulator : Unsupported controller type %d", int(new_type)); + return false; + } + + // Call the service + if (!m_client_switch_controllers.call(service)) + { + ROS_ERROR("Failed to call the service for switching controllers"); + success = false; + } + else + { + success = service.response.ok; + } + + // Update active type if the switch was successful + if (success) + { + m_active_controller_type = new_type; + } + } + + return success; +} + +kortex_driver::KortexError KortexArmSimulation::FillKortexError(uint32_t code, uint32_t subCode, const std::string& description) const +{ + kortex_driver::KortexError error; + error.code = code; + error.subCode = subCode; + error.description = description; + return error; +} + +void KortexArmSimulation::JoinThreadAndCancelAction() +{ + // Tell the thread to stop and join it + m_action_preempted = true; + if (m_action_executor_thread.joinable()) + { + m_action_executor_thread.join(); + } + m_current_action_type = 0; + m_action_preempted = false; +} + +void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) +{ + auto action_result = FillKortexError(kortex_driver::ErrorCodes::ERROR_NONE, kortex_driver::SubErrorCodes::SUB_ERROR_NONE); + + // Notify action started + kortex_driver::ActionNotification start_notif; + start_notif.handle = action.handle; + start_notif.action_event = kortex_driver::ActionEvent::ACTION_START; + m_pub_action_topic.publish(start_notif); + m_is_action_being_executed = true; + m_current_action_type = action.handle.action_type; + + // Switch executor on the action type + switch (action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + action_result = ExecuteReachJointAngles(action); + break; + case kortex_driver::ActionType::REACH_POSE: + action_result = ExecuteReachPose(action); + break; + case kortex_driver::ActionType::SEND_JOINT_SPEEDS: + action_result = ExecuteSendJointSpeeds(action); + break; + case kortex_driver::ActionType::SEND_TWIST_COMMAND: + action_result = ExecuteSendTwist(action); + break; + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + action_result = ExecuteSendGripperCommand(action); + break; + case kortex_driver::ActionType::TIME_DELAY: + action_result = ExecuteTimeDelay(action); + break; + default: + action_result = FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, kortex_driver::SubErrorCodes::UNSUPPORTED_ACTION); + break; + } + + // Oddly enough, gripper actions don't send notifications through Kortex API when they end + if (action.handle.action_type != kortex_driver::ActionType::SEND_GRIPPER_COMMAND) + { + kortex_driver::ActionNotification end_notif; + end_notif.handle = action.handle; + // Action was cancelled by user and is not a velocity command + if (m_action_preempted.load() && action.handle.action_type != kortex_driver::ActionType::SEND_JOINT_SPEEDS) + { + // Notify ACTION_ABORT + end_notif.action_event = kortex_driver::ActionEvent::ACTION_ABORT; + ROS_WARN("Action was aborted by user."); + } + // Action ended on its own + else + { + if (action_result.code != kortex_driver::ErrorCodes::ERROR_NONE) + { + // Notify ACTION_ABORT + end_notif.action_event = kortex_driver::ActionEvent::ACTION_ABORT; + end_notif.abort_details = action_result.subCode; + ROS_WARN("Action was failed : \nError code is %d\nSub-error code is %d\nError description is : %s", + action_result.code, + action_result.subCode, + action_result.description.c_str()); + } + else + { + // Notify ACTION_END + end_notif.action_event = kortex_driver::ActionEvent::ACTION_END; + } + } + m_pub_action_topic.publish(end_notif); + } + + m_is_action_being_executed = false; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const kortex_driver::Action& action) +{ + auto result = FillKortexError(kortex_driver::ErrorCodes::ERROR_NONE, kortex_driver::SubErrorCodes::SUB_ERROR_NONE); + if (action.oneof_action_parameters.reach_joint_angles.size() != 1) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing joint angles action : action is malformed."); + } + auto constrained_joint_angles = action.oneof_action_parameters.reach_joint_angles[0]; + if (constrained_joint_angles.joint_angles.joint_angles.size() != GetDOF()) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing joint angles action : action contains " + std::to_string(constrained_joint_angles.joint_angles.joint_angles.size()) + " joint angles but arm has " + std::to_string(GetDOF())); + } + + // Switch to trajectory controller + if (!SwitchControllerType(ControllerType::kTrajectory)) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::METHOD_FAILED, + "Error playing joint angles action : simulated trajectory controller could not be switched to."); + } + + // Initialize trajectory object + trajectory_msgs::JointTrajectory traj; + traj.header.frame_id = m_prefix + "base_link"; + for (int i = 0; i < constrained_joint_angles.joint_angles.joint_angles.size(); i++) + { + const std::string joint_name = m_prefix + "joint_" + std::to_string(i+1); //joint names are 1-based + traj.joint_names.push_back(joint_name); + } + + // Get current position + sensor_msgs::JointState current; + { + const std::lock_guard lock(m_state_mutex); + current = m_current_state; + } + + // Transform kortex structure to trajectory_msgs to fill endpoint structure + trajectory_msgs::JointTrajectoryPoint endpoint; + for (int i = 0; i < constrained_joint_angles.joint_angles.joint_angles.size(); i++) + { + // If the current actuator has turned on itself many times, we need the endpoint to follow that trend too + int n_turns = 0; + m_math_util.wrapRadiansFromMinusPiToPi(current.position[m_first_arm_joint_index + i], n_turns); + const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value)); + endpoint.positions.push_back(rad_wrapped_goal + double(n_turns) * 2.0 * M_PI); + endpoint.velocities.push_back(0.0); + endpoint.accelerations.push_back(0.0); + } + + // Calculate velocity profiles to know how much time this trajectory must last + switch (constrained_joint_angles.constraint.type) + { + // If the duration is supplied, set the duration of the velocity profiles with that value + case kortex_driver::JointTrajectoryConstraintType::JOINT_CONSTRAINT_DURATION: + { + // Error check on the given duration + if (constrained_joint_angles.constraint.value <= 0.0f) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Invalid duration constraint : it has to be higher than 0.0!"); + } + // Set the velocity profiles + for (int i = 0; i < GetDOF(); i++) + { + m_velocity_trap_profiles[i].SetProfileDuration(current.position[m_first_arm_joint_index + i], endpoint.positions[i], constrained_joint_angles.constraint.value); + } + endpoint.time_from_start = ros::Duration(constrained_joint_angles.constraint.value); + ROS_DEBUG("Using supplied duration : %2.2f", constrained_joint_angles.constraint.value); + break; + } + // If a max velocity is supplied for each joint, we need to find the limiting duration with this velocity constraint + case kortex_driver::JointTrajectoryConstraintType::JOINT_CONSTRAINT_SPEED: + { + // Error check on the given velocity + float max_velocity = m_math_util.toRad(constrained_joint_angles.constraint.value); + if (max_velocity <= 0.0f) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Invalid velocity constraint : it has to be higher than 0.0!"); + } + // Find the limiting duration with given velocity + double max_duration = 0.0; + for (int i = 0; i < GetDOF(); i++) + { + double velocity_ratio = std::min(1.0, double(max_velocity)/m_arm_velocity_max_limits[i]); + m_velocity_trap_profiles[i].SetProfileVelocity(current.position[m_first_arm_joint_index + i], endpoint.positions[i], velocity_ratio); + max_duration = std::max(max_duration, m_velocity_trap_profiles[i].Duration()); + ROS_DEBUG("Joint %d moving from %2.2f to %2.2f gives duration %2.2f", i, current.position[m_first_arm_joint_index + i], endpoint.positions[i], m_velocity_trap_profiles[i].Duration()); + } + ROS_DEBUG("max_duration is : %2.2f", max_duration); + // Set the velocity profiles + for (int i = 0; i < GetDOF(); i++) + { + m_velocity_trap_profiles[i].SetProfileDuration(current.position[m_first_arm_joint_index + i], endpoint.positions[i], max_duration); + } + endpoint.time_from_start = ros::Duration(max_duration); + break; + } + default: + { + // Find the optimal duration based on actual velocity and acceleration limits + double optimal_duration = 0.0; + for (int i = 0; i < GetDOF(); i++) + { + m_velocity_trap_profiles[i].SetProfile(current.position[m_first_arm_joint_index + i], endpoint.positions[i]); + optimal_duration = std::max(optimal_duration, m_velocity_trap_profiles[i].Duration()); + ROS_DEBUG("Joint %d moving from %2.2f to %2.2f gives duration %2.2f", i, current.position[m_first_arm_joint_index + i], endpoint.positions[i], m_velocity_trap_profiles[i].Duration()); + } + ROS_DEBUG("optimal_duration is : %2.2f", optimal_duration); + // Set the velocity profiles + for (int i = 0; i < GetDOF(); i++) + { + m_velocity_trap_profiles[i].SetProfileDuration(current.position[m_first_arm_joint_index + i], endpoint.positions[i], optimal_duration); + } + endpoint.time_from_start = ros::Duration(optimal_duration); + break; + } + } + + // Copy velocity profile data into trajectory using JOINT_TRAJECTORY_TIMESTEP_SECONDS timesteps + // For each timestep + for (double t = JOINT_TRAJECTORY_TIMESTEP_SECONDS; t < m_velocity_trap_profiles[0].Duration(); t += JOINT_TRAJECTORY_TIMESTEP_SECONDS) + { + // Create trajectory point + trajectory_msgs::JointTrajectoryPoint p; + p.time_from_start = ros::Duration(t); + // Add position, velocity, acceleration from each velocity profile + for (int i = 0; i < GetDOF(); i++) + { + p.positions.push_back(m_velocity_trap_profiles[i].Pos(t)); + p.velocities.push_back(m_velocity_trap_profiles[i].Vel(t)); + p.accelerations.push_back(m_velocity_trap_profiles[i].Acc(t)); + } + // Add trajectory point to goal + traj.points.push_back(p); + } + // Finally, add endpoint to trajectory + // Add position, velocity, acceleration from each velocity profile + trajectory_msgs::JointTrajectoryPoint p; + p.time_from_start = ros::Duration(m_velocity_trap_profiles[0].Duration()); + for (int i = 0; i < GetDOF(); i++) + { + p.positions.push_back(m_velocity_trap_profiles[i].Pos(m_velocity_trap_profiles[i].Duration())); + p.velocities.push_back(m_velocity_trap_profiles[i].Vel(m_velocity_trap_profiles[i].Duration())); + p.accelerations.push_back(m_velocity_trap_profiles[i].Acc(m_velocity_trap_profiles[i].Duration())); + } + // Add trajectory point to goal + traj.points.push_back(p); + + // Verify if goal has been cancelled before sending it + if (m_action_preempted.load()) + { + return result; + } + + // Send goal + control_msgs::FollowJointTrajectoryActionGoal goal; + traj.header.stamp = ros::Time::now(); + goal.goal.trajectory = traj; + m_follow_joint_trajectory_action_client->sendGoal(goal.goal); + + // Wait for goal to be done, or for preempt to be called (check every 100ms) + while(!m_action_preempted.load()) + { + if (m_follow_joint_trajectory_action_client->waitForResult(ros::Duration(0.1f))) + { + // Sometimes an error is thrown related to a bad cast in a ros::time structure inside the SimpleActionClient + // See https://answers.ros.org/question/209452/exception-thrown-while-processing-service-call-time-is-out-of-dual-32-bit-range/ + // If this error happens here we just send the goal again with an updated timestamp + auto status = m_follow_joint_trajectory_action_client->getResult(); + if (status->error_string == "Time is out of dual 32-bit range") + { + traj.header.stamp = ros::Time::now(); + goal.goal.trajectory = traj; + m_follow_joint_trajectory_action_client->sendGoal(goal.goal); + } + else + { + break; + } + } + } + + // If we got out of the loop because we're preempted, cancel the goal before returning + if (m_action_preempted.load()) + { + m_follow_joint_trajectory_action_client->cancelAllGoals(); + } + // Fill result depending on action final status if user didn't cancel + else + { + auto status = m_follow_joint_trajectory_action_client->getResult(); + if (status->error_code != status->SUCCESSFUL) + { + result = FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + status->error_string); + } + } + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteReachPose(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.reach_pose.size() != 1) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing pose action : action is malformed."); + } + auto constrained_pose = action.oneof_action_parameters.reach_pose[0]; + + // Switch to trajectory controller + if (!SwitchControllerType(ControllerType::kTrajectory)) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::METHOD_FAILED, + "Error playing pose action : simulated trajectory controller could not be switched to."); + } + + // Get current position + sensor_msgs::JointState current; + { + const std::lock_guard lock(m_state_mutex); + current = m_current_state; + } + + // Get Start frame + // For the Rotation part : ThetaX = gamma, ThetaY = beta, ThetaZ = alpha + auto start = KDL::Frame(); + Eigen::VectorXd positions_eigen(m_degrees_of_freedom); + for (int i = 0; i < GetDOF(); i++) + { + positions_eigen[i] = current.position[m_first_arm_joint_index + i]; + } + KDL::JntArray current_kdl(GetDOF()); + current_kdl.data = positions_eigen; + m_fk_solver->JntToCart(current_kdl, start); + + { + ROS_DEBUG("START FRAME :"); + ROS_DEBUG("X=%2.4f Y=%2.4f Z=%2.4f", start.p[0], start.p[1], start.p[2]); + double sa, sb, sg; start.M.GetEulerZYX(sa, sb, sg); + ROS_DEBUG("ALPHA=%2.4f BETA=%2.4f GAMMA=%2.4f", m_math_util.toDeg(sa), m_math_util.toDeg(sb), m_math_util.toDeg(sg)); + KDL::Vector axis; + ROS_DEBUG("start rot = %2.4f", start.M.GetRotAngle(axis)); + } + + // Get End frame + auto end_pos = KDL::Vector(constrained_pose.target_pose.x, constrained_pose.target_pose.y, constrained_pose.target_pose.z); + auto end_rot = KDL::Rotation::EulerZYX(m_math_util.toRad(constrained_pose.target_pose.theta_z), m_math_util.toRad(constrained_pose.target_pose.theta_y), m_math_util.toRad(constrained_pose.target_pose.theta_x)); + KDL::Frame end(end_rot, end_pos); + + { + ROS_DEBUG("END FRAME :"); + ROS_DEBUG("X=%2.4f Y=%2.4f Z=%2.4f", end_pos[0], end_pos[1], end_pos[2]); + double ea, eb, eg; end_rot.GetEulerZYX(ea, eb, eg); + ROS_DEBUG("ALPHA=%2.4f BETA=%2.4f GAMMA=%2.4f", m_math_util.toDeg(ea), m_math_util.toDeg(eb), m_math_util.toDeg(eg)); + KDL::Vector axis; + ROS_DEBUG("end rot = %2.4f", end_rot.GetRotAngle(axis)); + } + + // If different speed limits than the default ones are provided, use them instead + float translation_speed_limit = m_max_cartesian_twist_linear; + float rotation_speed_limit = m_max_cartesian_twist_angular; + if (!constrained_pose.constraint.oneof_type.speed.empty()) + { + // If a max velocity is supplied for each joint, we need to find the limiting duration with this velocity constraint + translation_speed_limit = std::min(constrained_pose.constraint.oneof_type.speed[0].translation, translation_speed_limit); + rotation_speed_limit = std::min(constrained_pose.constraint.oneof_type.speed[0].orientation, rotation_speed_limit); + } + + // Calculate norm of translation movement and minimum duration to move this amount given max translation speed + double delta_pos = (end_pos - start.p).Norm(); + double minimum_translation_duration = delta_pos / m_max_cartesian_twist_linear; // in seconds + + // Calculate angle variation of rotation movement and minimum duration to move this amount given max rotation speed + KDL::Vector axis; // we need to create this variable to access the RotAngle for start and end frames'rotation components + KDL::Rotation dR = end_rot * start.M.Inverse(); + double delta_rot = dR.GetRotAngle(axis); + double minimum_rotation_duration = delta_rot / m_max_cartesian_twist_angular; // in seconds + + ROS_INFO("trans : %2.4f rot : %2.4f", minimum_translation_duration, minimum_rotation_duration); + + // The default value for the duration will be the longer duration of the two + double duration = std::max(minimum_translation_duration, minimum_rotation_duration); + + // eq_radius is chosen here to make it so translations and rotations are normalised + // Here is a good explanation for it : https://github.com/zakharov/BRICS_RN/blob/master/navigation_trajectory_common/include/navigation_trajectory_common/Conversions.h#L358-L382 + float eq_radius = translation_speed_limit / rotation_speed_limit; + + // Create Path_Line object + // I know this is ugly but the RotationalInterpolation object is mandatory and needs to be created as such + KDL::Path_Line line(start, end, new KDL::RotationalInterpolation_SingleAxis(), eq_radius); + + // Create a trapezoidal velocity profile for the Cartesian trajectory to parametrize it in time + KDL::VelocityProfile_Trap velocity_profile(std::min(translation_speed_limit, rotation_speed_limit), m_max_cartesian_acceleration_linear); + velocity_profile.SetProfile(0.0, line.PathLength()); + duration = std::max(duration, velocity_profile.Duration()); + + // If duration is not supplied, use the one we just calculated + // If the duration is supplied, simply use it + if (!constrained_pose.constraint.oneof_type.duration.empty()) + { + double supplied_duration = constrained_pose.constraint.oneof_type.duration[0]; + if (duration > supplied_duration) + { + ROS_WARN("Cannot use supplied duration %2.4f because the minimum duration based on velocity limits is %2.4f", + supplied_duration, + duration); + } + duration = std::max(duration, supplied_duration); + } + + // Set the velocity profile duration + velocity_profile.SetProfileDuration(0.0, line.PathLength(), duration); + KDL::Trajectory_Segment segment(&line, &velocity_profile, false); + ROS_DEBUG("Duration of trajectory will be %2.4f seconds", duration); + + // Initialize trajectory object + trajectory_msgs::JointTrajectory traj; + traj.header.frame_id = m_prefix + "base_link"; + traj.header.stamp = ros::Time::now(); + for (int i = 0; i < GetDOF(); i++) + { + const std::string joint_name = m_prefix + "joint_" + std::to_string(i+1); //joint names are 1-based + traj.joint_names.push_back(joint_name); + } + + // Fill trajectory object + KDL::JntArray previous = current_kdl; // Position i - 1, initialise to starting angles + KDL::JntArray current_joints(GetDOF()); // Position i + for (float t = JOINT_TRAJECTORY_TIMESTEP_SECONDS; t < duration; t += JOINT_TRAJECTORY_TIMESTEP_SECONDS) + { + // First get the next Cartesian position and twists + auto pos = segment.Pos(t); + + // Position is enough to have a smooth trajectory it seems but if eventually vel and acc are useful somehow this is how to get them: + // auto vel = segment.Vel(t); + // auto acc = segment.Acc(t); + + // Create trajectory point + trajectory_msgs::JointTrajectoryPoint p; + p.time_from_start = ros::Duration(t); + + // Use inverse IK solver + int code = m_ik_pos_solver->CartToJnt(previous, pos, current_joints); + if (code != m_ik_pos_solver->E_NOERROR) + { + ROS_ERROR("IK ERROR CODE = %d", code); + } + + for (int i = 0; i < GetDOF(); i++) + { + p.positions.push_back(current_joints(i)); + } + + // Add trajectory point to goal + traj.points.push_back(p); + previous = current_joints; + } + + // Add last point + trajectory_msgs::JointTrajectoryPoint p; + p.time_from_start = ros::Duration(segment.Duration()); + auto pos = segment.Pos(segment.Duration()); + int code = m_ik_pos_solver->CartToJnt(previous, pos, current_joints); + for (int i = 0; i < GetDOF(); i++) + { + p.positions.push_back(current_joints(i)); + } + + // Add trajectory point to goal + traj.points.push_back(p); + + // Verify if goal has been cancelled before sending it + if (m_action_preempted.load()) + { + return result; + } + + // Send goal + control_msgs::FollowJointTrajectoryActionGoal goal; + goal.goal.trajectory = traj; + m_follow_joint_trajectory_action_client->sendGoal(goal.goal); + + // Wait for goal to be done, or for preempt to be called (check every 10ms) + while(!m_action_preempted.load() && !m_follow_joint_trajectory_action_client->waitForResult(ros::Duration(0.01f))) {} + + // If we got out of the loop because we're preempted, cancel the goal before returning + if (m_action_preempted.load()) + { + m_follow_joint_trajectory_action_client->cancelAllGoals(); + } + // Fill result depending on action final status if user didn't cancel + else + { + auto status = m_follow_joint_trajectory_action_client->getResult(); + if (status->error_code != status->SUCCESSFUL) + { + result = FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + status->error_string); + } + } + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_joint_speeds.size() != 1) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing joints speeds : action is malformed."); + } + auto joint_speeds = action.oneof_action_parameters.send_joint_speeds[0]; + if (joint_speeds.joint_speeds.size() != GetDOF()) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing joint speeds action : action contains " + std::to_string(joint_speeds.joint_speeds.size()) + " joint speeds but arm has " + std::to_string(GetDOF())); + } + + // Switch to trajectory controller + if (!SwitchControllerType(ControllerType::kIndividual)) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::METHOD_FAILED, + "Error playing joint speeds action : simulated positions controllers could not be switched to."); + } + + // Get current position + sensor_msgs::JointState current; + { + const std::lock_guard lock(m_state_mutex); + current = m_current_state; + } + + // Initialise commands + std::vector commands(GetDOF(), 0.0); // in rad + for (int i = 0; i < GetDOF(); i++) + { + commands[i] = current.position[m_first_arm_joint_index + i]; + } + std::vector previous_commands = commands; // in rad + std::vector velocity_commands(GetDOF(), 0.0); // in rad/s + std::vector previous_velocity_commands(GetDOF(), 0.0); // in rad/s + std::vector stopped(GetDOF(), false); + + // While we're not done + while (true) + { + // If action is preempted, set the velocities to 0 + if (m_action_preempted.load()) + { + std::fill(m_velocity_commands.begin(), m_velocity_commands.end(), 0.0); + } + // For each joint + for (int i = 0; i < GetDOF(); i++) + { + // Calculate real position increment + // This helps to know if we hit joint limits or if we stopped + { + const std::lock_guard lock(m_state_mutex); + current = m_current_state; + } + + // Calculate permitted velocity command because we don't have infinite acceleration + double vel_delta = m_velocity_commands[i] - previous_velocity_commands[i]; + double max_delta = std::copysign(m_arm_acceleration_max_limits[i] * JOINT_TRAJECTORY_TIMESTEP_SECONDS, vel_delta); + + // If the velocity change is within acceleration limits for this timestep + double velocity_command; + if (fabs(vel_delta) < fabs(max_delta)) + { + velocity_command = m_velocity_commands[i]; + } + // If we cannot instantly accelerate to this velocity + else + { + velocity_command = previous_velocity_commands[i] + max_delta; + } + + // Cap to the velocity limit for the joint + velocity_command = std::copysign(std::min(fabs(velocity_command), double(fabs(m_arm_velocity_max_limits[i]))), velocity_command); + + // Check if velocity command is in fact too small + if (fabs(velocity_command) < MINIMUM_JOINT_VELOCITY_RAD_PER_SECONDS) + { + velocity_command = 0.0; + commands[i] = current.position[m_first_arm_joint_index + i]; + stopped[i] = true; + } + // Else calculate the position increment and send it + else + { + commands[i] = previous_commands[i] + velocity_command * JOINT_TRAJECTORY_TIMESTEP_SECONDS; + stopped[i] = false; + + // Cap the command to the joint limit + if (m_arm_joint_limits_min[i] != 0.0 && commands[i] < 0.0) + { + commands[i] = std::max(m_arm_joint_limits_min[i], commands[i]); + velocity_command = 0.0; + } + else if (m_arm_joint_limits_max[i] != 0.0 && commands[i] > 0.0) + { + commands[i] = std::min(m_arm_joint_limits_max[i], commands[i]); + velocity_command = 0.0; + } + + // Send the position increments to the controllers + std_msgs::Float64 message; + message.data = commands[i]; + m_pub_position_controllers[i].publish(message); + } + + // Remember actual command as previous and actual velocity command as previous + previous_commands[i] = commands[i]; + previous_velocity_commands[i] = velocity_command; + } + + // Sleep for TIMESTEP + std::this_thread::sleep_for(std::chrono::milliseconds(int(1000*JOINT_TRAJECTORY_TIMESTEP_SECONDS))); + + // If the action is preempted and we're back to zero velocity, we're done here + if (m_action_preempted.load()) + { + // Check if all joints are stopped and break if yes + if (std::all_of(stopped.begin(), stopped.end(), [](bool b){return b;})) + { + break; + } + } + } + + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteSendTwist(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_twist_command.size() != 1) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing twist action : action is malformed."); + } + auto twist = action.oneof_action_parameters.send_twist_command[0]; + + // Switch to trajectory controller + if (!SwitchControllerType(ControllerType::kIndividual)) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::METHOD_FAILED, + "Error playing joint speeds action : simulated positions controllers could not be switched to."); + } + + // Only mixed frame is supported in simulation + if (twist.reference_frame != kortex_driver::CartesianReferenceFrame::CARTESIAN_REFERENCE_FRAME_MIXED) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing twist action : only mixed frame is supported in simulation."); + } + + // Get current position + sensor_msgs::JointState current; + { + const std::lock_guard lock(m_state_mutex); + current = m_current_state; + } + + // Initialise commands + std::vector commands(GetDOF(), 0.0); // in rad + for (int i = 0; i < GetDOF(); i++) + { + commands[i] = current.position[m_first_arm_joint_index + i]; + } + std::vector previous_commands = commands; // in rad + std::vector previous_velocity_commands(GetDOF(), 0.0); // in rad/s + std::vector stopped(GetDOF(), false); + kortex_driver::Twist twist_command = m_twist_command; + kortex_driver::Twist previous_twist_command; + + // While we're not done + while (ros::ok()) + { + // If action is preempted, set the velocities to 0 + if (m_action_preempted.load()) + { + m_twist_command = kortex_driver::Twist(); + } + + // Calculate actual twist command considering max linear and angular accelerations + double max_linear_twist_delta = JOINT_TRAJECTORY_TIMESTEP_SECONDS * m_max_cartesian_acceleration_linear; + double max_angular_twist_delta = JOINT_TRAJECTORY_TIMESTEP_SECONDS * m_max_cartesian_acceleration_angular; + kortex_driver::Twist delta_twist = m_math_util.substractTwists(m_twist_command, previous_twist_command); + + // If the velocity change is within acceleration limits for this timestep + if (fabs(delta_twist.linear_x) < fabs(max_linear_twist_delta)) + { + twist_command.linear_x = m_twist_command.linear_x; + } + // If we cannot instantly accelerate to this velocity + else + { + twist_command.linear_x = previous_twist_command.linear_x + std::copysign(max_linear_twist_delta, delta_twist.linear_x); + } + // same for linear_y + if (fabs(delta_twist.linear_y) < fabs(max_linear_twist_delta)) + { + twist_command.linear_y = m_twist_command.linear_y; + } + else + { + twist_command.linear_y = previous_twist_command.linear_y + std::copysign(max_linear_twist_delta, delta_twist.linear_y); + } + // same for linear_z + if (fabs(delta_twist.linear_z) < fabs(max_linear_twist_delta)) + { + twist_command.linear_z = m_twist_command.linear_z; + } + else + { + twist_command.linear_z = previous_twist_command.linear_z + std::copysign(max_linear_twist_delta, delta_twist.linear_z); + } + // same for angular_x + if (fabs(delta_twist.angular_x) < fabs(max_angular_twist_delta)) + { + twist_command.angular_x = m_twist_command.angular_x; + } + else + { + twist_command.angular_x = previous_twist_command.angular_x + std::copysign(max_angular_twist_delta, delta_twist.angular_x); + } + // same for angular_y + if (fabs(delta_twist.angular_y) < fabs(max_angular_twist_delta)) + { + twist_command.angular_y = m_twist_command.angular_y; + } + else + { + twist_command.angular_y = previous_twist_command.angular_y + std::copysign(max_angular_twist_delta, delta_twist.angular_y); + } + // same for angular_z + if (fabs(delta_twist.angular_z) < fabs(max_angular_twist_delta)) + { + twist_command.angular_z = m_twist_command.angular_z; + } + else + { + twist_command.angular_z = previous_twist_command.angular_z + std::copysign(max_angular_twist_delta, delta_twist.angular_z); + } + + // Cap to the velocity limit + twist_command.linear_x = std::copysign(std::min(fabs(twist_command.linear_x), fabs(m_max_cartesian_twist_linear)), twist_command.linear_x); + twist_command.linear_y = std::copysign(std::min(fabs(twist_command.linear_y), fabs(m_max_cartesian_twist_linear)), twist_command.linear_y); + twist_command.linear_z = std::copysign(std::min(fabs(twist_command.linear_z), fabs(m_max_cartesian_twist_linear)), twist_command.linear_z); + twist_command.angular_x = std::copysign(std::min(fabs(twist_command.angular_x), fabs(m_max_cartesian_twist_angular)), twist_command.angular_x); + twist_command.angular_y = std::copysign(std::min(fabs(twist_command.angular_y), fabs(m_max_cartesian_twist_angular)), twist_command.angular_y); + twist_command.angular_z = std::copysign(std::min(fabs(twist_command.angular_z), fabs(m_max_cartesian_twist_angular)), twist_command.angular_z); + + // Fill current joint position commands KDL structure + KDL::JntArray commands_kdl(GetDOF()); + Eigen::VectorXd commands_eigen(GetDOF()); + for (int i = 0; i < GetDOF(); i++) + { + commands_eigen[i] = commands[i]; + } + commands_kdl.data = commands_eigen; + + // Fill KDL Twist structure with Kortex twist + KDL::Twist twist_kdl = KDL::Twist(KDL::Vector(twist_command.linear_x, twist_command.linear_y, twist_command.linear_z), + KDL::Vector(twist_command.angular_x, twist_command.angular_y, twist_command.angular_z)); + + // Call IK and fill joint velocity commands + KDL::JntArray joint_velocities(GetDOF()); + int ik_result = m_ik_vel_solver->CartToJnt(commands_kdl, twist_kdl, joint_velocities); + if (ik_result != m_ik_vel_solver->E_NOERROR) + { + ROS_WARN("IK ERROR = %d", ik_result); + } + + // We need to know if the joint velocities have to be adjusted, and by what ratio + double ratio = 1.0; + for (int i = 0; i < GetDOF(); i++) + { + // Calculate permitted velocity command because we don't have infinite acceleration + double vel_delta = joint_velocities(i) - previous_velocity_commands[i]; + double max_delta = std::copysign(m_arm_acceleration_max_limits[i] * JOINT_TRAJECTORY_TIMESTEP_SECONDS, vel_delta); + + // If we cannot instantly accelerate to this velocity + if (fabs(vel_delta) > fabs(max_delta)) + { + ratio = std::max(ratio, fabs(vel_delta / max_delta)); + } + } + + // Command the velocities + // For each joint + for (int i = 0; i < GetDOF(); i++) + { + // Calculate position increment + commands[i] = previous_commands[i] + joint_velocities(i) / ratio * JOINT_TRAJECTORY_TIMESTEP_SECONDS; + + // Cap the command to the joint limit + if (m_arm_joint_limits_min[i] != 0.0 && commands[i] < 0.0) + { + commands[i] = std::max(m_arm_joint_limits_min[i], commands[i]); + } + else if (m_arm_joint_limits_max[i] != 0.0 && commands[i] > 0.0) + { + commands[i] = std::min(m_arm_joint_limits_max[i], commands[i]); + } + + // Send the position increments to the controllers + std_msgs::Float64 message; + message.data = commands[i]; + m_pub_position_controllers[i].publish(message); + + // Check if joint is stopped or not + stopped[i] = fabs(joint_velocities(i)) < MINIMUM_JOINT_VELOCITY_RAD_PER_SECONDS; + + // Remember actual command as previous + previous_commands[i] = commands[i]; + previous_velocity_commands[i] = joint_velocities(i); + } + + // Put current values to previous + previous_twist_command = twist_command; + + // Sleep for TIMESTEP + std::this_thread::sleep_for(std::chrono::milliseconds(int(1000*JOINT_TRAJECTORY_TIMESTEP_SECONDS))); + + // If the action is preempted and we're back to zero velocity, we're done here + if (m_action_preempted.load()) + { + // Check if all joints are stopped and break if yes + if (std::all_of(stopped.begin(), stopped.end(), [](bool b){return b;})) + { + break; + } + } + } + + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteSendGripperCommand(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_gripper_command.size() != 1) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing gripper command action : action is malformed."); + } + auto gripper_command = action.oneof_action_parameters.send_gripper_command[0]; + + if (gripper_command.gripper.finger.size() != 1) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Error playing gripper command action : there must be exactly one finger"); + } + + if (gripper_command.mode != kortex_driver::GripperMode::GRIPPER_POSITION) + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::UNSUPPORTED_ACTION, + "Error playing gripper command action : gripper mode " + std::to_string(gripper_command.mode) + " is not supported; only position is."); + } + + // The incoming command is relative [0,1] and we need to put it in absolute unit [m_gripper_joint_limits_min[0], m_gripper_joint_limits_max[0]]: + double absolute_gripper_command = m_math_util.absolute_position_from_relative(gripper_command.gripper.finger[0].value, m_gripper_joint_limits_min[0], m_gripper_joint_limits_max[0]); + + // Create the goal + control_msgs::GripperCommandGoal goal; + goal.command.position = absolute_gripper_command; + + // Verify if goal has been cancelled before sending it + if (m_action_preempted.load()) + { + return result; + } + + // Send goal + m_gripper_action_client->sendGoal(goal); + + // Wait for goal to be done, or for preempt to be called (check every 10ms) + while(!m_action_preempted.load() && !m_gripper_action_client->waitForResult(ros::Duration(0.01f))) {} + + // If we got out of the loop because we're preempted, cancel the goal before returning + if (m_action_preempted.load()) + { + m_gripper_action_client->cancelAllGoals(); + } + // Fill result depending on action final status if user didn't cancel + else + { + auto status = m_gripper_action_client->getResult(); + + if (!status->reached_goal) + { + result = FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::METHOD_FAILED, + "The gripper command failed during execution."); + } + } + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteTimeDelay(const kortex_driver::Action& action) +{ + auto result = FillKortexError(kortex_driver::ErrorCodes::ERROR_NONE, + kortex_driver::SubErrorCodes::SUB_ERROR_NONE); + if (!action.oneof_action_parameters.delay.empty()) + { + auto start = std::chrono::system_clock::now(); + uint32_t delay_seconds = action.oneof_action_parameters.delay[0].duration; + // While not preempted and duration not elapsed + while (!m_action_preempted.load() && (std::chrono::system_clock::now() - start) < std::chrono::seconds(delay_seconds)) + { + // sleep a bit + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + else + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing time delay action : action is malformed."; + } + return result; +} + +void KortexArmSimulation::new_joint_speeds_cb(const kortex_driver::Base_JointSpeeds& joint_speeds) +{ + kortex_driver::SendJointSpeedsCommandRequest req; + req.input = joint_speeds; + SendJointSpeedsCommand(req); +} + +void KortexArmSimulation::new_twist_cb(const kortex_driver::TwistCommand& twist) +{ + // TODO Implement +} + +void KortexArmSimulation::clear_faults_cb(const std_msgs::Empty& empty) +{ + // does nothing +} + +void KortexArmSimulation::stop_cb(const std_msgs::Empty& empty) +{ + Stop(kortex_driver::StopRequest()); +} + +void KortexArmSimulation::emergency_stop_cb(const std_msgs::Empty& empty) +{ + ApplyEmergencyStop(kortex_driver::ApplyEmergencyStopRequest()); +} diff --git a/kortex_driver/src/non-generated/driver/kortex_math_util.cpp b/kortex_driver/src/non-generated/driver/kortex_math_util.cpp index 4f140ce0..e89552bd 100644 --- a/kortex_driver/src/non-generated/driver/kortex_math_util.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_math_util.cpp @@ -22,17 +22,32 @@ double KortexMathUtil::toDeg(double rad) return rad * 180.0 / M_PI; } +int KortexMathUtil::getNumberOfTurns(double rad_not_wrapped) +{ + // it is between + return 0; +} + double KortexMathUtil::wrapRadiansFromMinusPiToPi(double rad_not_wrapped) +{ + int n; + return wrapRadiansFromMinusPiToPi(rad_not_wrapped, n); +} + +double KortexMathUtil::wrapRadiansFromMinusPiToPi(double rad_not_wrapped, int& number_of_turns) { bool properly_wrapped = false; + number_of_turns = 0; do { if (rad_not_wrapped > M_PI) { + number_of_turns += 1; rad_not_wrapped -= 2.0*M_PI; } else if (rad_not_wrapped < -M_PI) { + number_of_turns -= 1; rad_not_wrapped += 2.0*M_PI; } else @@ -44,16 +59,25 @@ double KortexMathUtil::wrapRadiansFromMinusPiToPi(double rad_not_wrapped) } double KortexMathUtil::wrapDegreesFromZeroTo360(double deg_not_wrapped) +{ + int n; + return wrapDegreesFromZeroTo360(deg_not_wrapped, n); +} + +double KortexMathUtil::wrapDegreesFromZeroTo360(double deg_not_wrapped, int& number_of_turns) { bool properly_wrapped = false; + number_of_turns = 0; do { if (deg_not_wrapped > 360.0) { + number_of_turns += 1; deg_not_wrapped -= 360.0; } else if (deg_not_wrapped < 0.0) { + number_of_turns -= 1; deg_not_wrapped += 360.0; } else @@ -75,3 +99,15 @@ double KortexMathUtil::absolute_position_from_relative(double relative_position, double range = max_value - min_value; return relative_position * range + min_value; } + +kortex_driver::Twist KortexMathUtil::substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b) +{ + kortex_driver::Twist c; + c.linear_x = a.linear_x - b.linear_x; + c.linear_y = a.linear_y - b.linear_y; + c.linear_z = a.linear_z - b.linear_z; + c.angular_x = a.angular_x - b.angular_x; + c.angular_y = a.angular_y - b.angular_y; + c.angular_z = a.angular_z - b.angular_z; + return c; +} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc b/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc index 42ac5233..60ec3c50 100644 --- a/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc @@ -54,15 +54,3 @@ TEST_F(KortexDriverTest, parseURDF) urdf::Model model; ASSERT_TRUE(model.initParam("robot_description")); } - -int main(int argc, char** argv){ - ros::init(argc, argv, "KortexArmDriverInitTestsNode"); - testing::InitGoogleTest(&argc, argv); - - std::thread t([]{while(ros::ok()) ros::spin();}); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - return res; -} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc new file mode 100644 index 00000000..9b05c1fb --- /dev/null +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -0,0 +1,508 @@ +#include +#include +#include +#include +#include +#include "kortex_driver/ActionEvent.h" +#include "kortex_driver/CartesianReferenceFrame.h" +#include "kortex_driver/GripperMode.h" + +class KortexSimulatorTest : public ::testing::Test { + protected: + + void SetUp() override + { + // Create Simulator + m_simulator.reset(new KortexArmSimulation(n)); + + // Create dummy action + dummy_action.name = "MyDummyAction"; + dummy_action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + dummy_action.handle.permission = 7; + kortex_driver::ConstrainedJointAngles angles; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = 10.0f*i; + angles.joint_angles.joint_angles.push_back(angle); + } + dummy_action.oneof_action_parameters.reach_joint_angles.push_back(angles); + + // Create action topic subscriber + m_action_topic_sub = n.subscribe("action_topic", 5, &KortexSimulatorTest::ActionTopicHandler, this); + m_received_notifications.clear(); + } + + void TearDown() override + { + } + + void CompareReachJointAnglesActions(const kortex_driver::Action& a1, const kortex_driver::Action& a2, bool same) + { + ASSERT_EQ(a1.oneof_action_parameters.reach_joint_angles.size(), a2.oneof_action_parameters.reach_joint_angles.size()); + auto angles1 = a1.oneof_action_parameters.reach_joint_angles[0]; + auto angles2 = a2.oneof_action_parameters.reach_joint_angles[0]; + ASSERT_EQ(angles1.joint_angles.joint_angles.size(), angles2.joint_angles.joint_angles.size()); + for (int i = 0; i < angles1.joint_angles.joint_angles.size() && same; i++) + { + ASSERT_EQ(same, angles1.joint_angles.joint_angles.at(i) == angles2.joint_angles.joint_angles.at(i)); + } + } + + void ActionTopicHandler(const kortex_driver::ActionNotification& notif) + { + m_received_notifications.push_back(notif); + } + + kortex_driver::Action dummy_action; + ros::NodeHandle n; + std::unique_ptr m_simulator; + ros::Subscriber m_action_topic_sub; + std::vector m_received_notifications; +}; + +// Make sure after initialisation the default actions are +// Tests ReadAllActions at the same time +TEST_F(KortexSimulatorTest, DefaultActions) +{ + auto actions = m_simulator->GetActionsMap(); + bool retract, home, zero, other = false; + for (auto a : actions) + { + if (a.second.name == "Retract") retract = true; + else if (a.second.name == "Home") home = true; + else if (a.second.name == "Zero") zero = true; + else other = true; + } + ASSERT_TRUE(retract); + ASSERT_TRUE(home); + ASSERT_TRUE(zero); + ASSERT_FALSE(other); +} + +// Tests DeleteAction so default actions are not deleted +TEST_F(KortexSimulatorTest, DeleteDefaultActions) +{ + static const std::vector DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + // Make sure the action can be deleted properly + kortex_driver::DeleteAction::Request req; + kortex_driver::ActionHandle handle; + for (unsigned int i : DEFAULT_ACTIONS_IDENTIFIERS) + { + handle.identifier = i; + req.input = handle; + m_simulator->DeleteAction(req); + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(i), 1); + } +} + +// Tests UpdateAction so default actions are not updated +TEST_F(KortexSimulatorTest, UpdateDefaultActions) +{ + static const std::vector DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + // Make sure the action cannot be updated + kortex_driver::UpdateAction::Request req; + for (unsigned int i : DEFAULT_ACTIONS_IDENTIFIERS) + { + dummy_action.handle.identifier = i; + req.input = dummy_action; + m_simulator->UpdateAction(req); + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(i), 1); + CompareReachJointAnglesActions(actions_map[i], dummy_action, false); + } +} + +// Tests ReadAllActions +TEST_F(KortexSimulatorTest, ReadAllActions) +{ + // Test for 3 known actions (default) of this type in the map + kortex_driver::ReadAllActions::Request req; + kortex_driver::RequestedActionType type; + type.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + req.input = type; + auto res = m_simulator->ReadAllActions(req); + auto action_list = res.output; + ASSERT_EQ(action_list.action_list.size(), 3); // Number of default actions + + // Test for 0 known actions of this type in the map + type.action_type = kortex_driver::ActionType::REACH_POSE; + req.input = type; + res = m_simulator->ReadAllActions(req); + action_list = res.output; + ASSERT_TRUE(action_list.action_list.empty()); +} + +// Tests CreateAction handler for a supported Action, and DeleteAction +TEST_F(KortexSimulatorTest, CreateSupportedAction) +{ + static const std::string name = "MyNewAction"; + + kortex_driver::CreateAction::Request req; + dummy_action.name = name; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(actions_map[handle.identifier].name, name); + ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); + + // Make sure the action can be deleted properly + kortex_driver::DeleteAction::Request del_req; + del_req.input = handle; + m_simulator->DeleteAction(del_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 0); +} + +// Tests CreateAction handler for an unsupported Action +TEST_F(KortexSimulatorTest, CreateUnsupportedAction) +{ + kortex_driver::CreateAction::Request req; + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + kortex_driver::Base_JointSpeeds speeds; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointSpeed speed; + speed.joint_identifier = i; + speed.value = 10.0f*i; + speeds.joint_speeds.push_back(speed); + } + dummy_action.oneof_action_parameters.send_joint_speeds.push_back(speeds); + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 0); +} + +// Tests UpdateAction on existing and non-existing actions +TEST_F(KortexSimulatorTest, UpdateAction) +{ + // Create Action at first + kortex_driver::CreateAction::Request req; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); + + // Modify and update the Action + dummy_action.name = "MyUpdatedName"; + dummy_action.handle.identifier = handle.identifier; + dummy_action.oneof_action_parameters.reach_joint_angles[0].joint_angles.joint_angles[3].value = 0.0f; + kortex_driver::UpdateAction::Request update_req; + update_req.input = dummy_action; + m_simulator->UpdateAction(update_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); + CompareReachJointAnglesActions(dummy_action, actions_map[handle.identifier], true); + + // Modify and update the Action with a different type + kortex_driver::Action wrong_type_action; + wrong_type_action.name = "WrongType"; + wrong_type_action.handle.identifier = handle.identifier; + wrong_type_action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + wrong_type_action.oneof_action_parameters.reach_joint_angles.clear(); + wrong_type_action.oneof_action_parameters.send_joint_speeds.push_back(kortex_driver::Base_JointSpeeds()); + update_req.input = wrong_type_action; + m_simulator->UpdateAction(update_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); + ASSERT_EQ(kortex_driver::ActionType::REACH_JOINT_ANGLES, actions_map[handle.identifier].handle.action_type); +} + +// This uses a TIME_DELAY action to test execution +TEST_F(KortexSimulatorTest, ExecuteAction) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Make sure after a couple seconds we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + std::this_thread::sleep_for(std::chrono::seconds(3)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// This uses a TIME_DELAY action to test execution from reference +TEST_F(KortexSimulatorTest, ExecuteActionFromReference) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create the Delay action + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::CreateAction::Request req; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + dummy_action.handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(dummy_action.handle.identifier), 1); + + // Execute the Delay action by reference + ASSERT_EQ(m_received_notifications.size(), 0); + kortex_driver::ExecuteActionFromReference::Request execute_req; + execute_req.input = dummy_action.handle; + m_simulator->ExecuteActionFromReference(execute_req); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Make sure after a couple seconds we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + std::this_thread::sleep_for(std::chrono::seconds(3)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// This uses a TIME_DELAY action to test aborting +TEST_F(KortexSimulatorTest, StopAction) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare StopAction request + kortex_driver::StopAction::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->StopAction(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} + +TEST_F(KortexSimulatorTest, PlayCartesianTrajectory) +{ + kortex_driver::PlayCartesianTrajectory::Request req; + kortex_driver::ConstrainedPose pose; + pose.target_pose.x = 0.1; + pose.target_pose.y = 0.1; + pose.target_pose.z = 0.1; + pose.target_pose.theta_x = 0.1; + pose.target_pose.theta_y = 0.1; + pose.target_pose.theta_z = 0.1; + req.input = pose; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->PlayCartesianTrajectory(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendTwistCommand) +{ + kortex_driver::SendTwistCommand::Request req; + kortex_driver::TwistCommand twist_command; + twist_command.reference_frame = kortex_driver::CartesianReferenceFrame::CARTESIAN_REFERENCE_FRAME_BASE; + twist_command.twist.linear_x = 0.1; + twist_command.twist.linear_y = 0.1; + twist_command.twist.linear_z = 0.1; + twist_command.twist.angular_x = 0.1; + twist_command.twist.angular_y = 0.1; + twist_command.twist.angular_z = 0.1; + req.input = twist_command; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendTwistCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, PlayJointTrajectory) +{ + kortex_driver::PlayJointTrajectory::Request req; + kortex_driver::ConstrainedJointAngles constrained_joint_angles; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = i*10.0f; + constrained_joint_angles.joint_angles.joint_angles.push_back(angle); + } + req.input = constrained_joint_angles; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->PlayJointTrajectory(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendJointSpeedsCommand) +{ + kortex_driver::SendJointSpeedsCommand::Request req; + kortex_driver::Base_JointSpeeds joint_speeds; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointSpeed speed; + speed.joint_identifier = i; + speed.value = i*10.0f; + joint_speeds.joint_speeds.push_back(speed); + } + req.input = joint_speeds; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendJointSpeedsCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendGripperCommand) +{ + kortex_driver::SendGripperCommand::Request req; + kortex_driver::GripperCommand gripper_command; + gripper_command.mode = kortex_driver::GripperMode::GRIPPER_POSITION; + kortex_driver::Finger finger; + finger.finger_identifier = 0; + finger.value = 10.0f; + gripper_command.gripper.finger.push_back(finger); + req.input = gripper_command; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendGripperCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// Use a TIME_DELAY action to test the Stop RPC +TEST_F(KortexSimulatorTest, Stop) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare Stop request + kortex_driver::Stop::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->Stop(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} + +TEST_F(KortexSimulatorTest, ApplyEmergencyStop) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare Stop request + kortex_driver::ApplyEmergencyStop::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->ApplyEmergencyStop(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} diff --git a/kortex_driver/src/non-generated/tests/main.cc b/kortex_driver/src/non-generated/tests/main.cc new file mode 100644 index 00000000..d330136e --- /dev/null +++ b/kortex_driver/src/non-generated/tests/main.cc @@ -0,0 +1,15 @@ +#include +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "TestNode"); + testing::InitGoogleTest(&argc, argv); + + std::thread t([]{while(ros::ok()) ros::spin();}); + + auto res = RUN_ALL_TESTS(); + + ros::shutdown(); + return res; +} \ No newline at end of file diff --git a/kortex_driver/templates/services.h.jinja2 b/kortex_driver/templates/services_interface.h.jinja2 similarity index 52% rename from kortex_driver/templates/services.h.jinja2 rename to kortex_driver/templates/services_interface.h.jinja2 index 1199f9dd..9350d79f 100644 --- a/kortex_driver/templates/services.h.jinja2 +++ b/kortex_driver/templates/services_interface.h.jinja2 @@ -14,8 +14,8 @@ * This file has been auto-generated and should not be modified. */ -#ifndef _KORTEX_{{package.short_name|upper}}_SERVICES_H_ -#define _KORTEX_{{package.short_name|upper}}_SERVICES_H_ +#ifndef _KORTEX_{{package.short_name|upper}}_SERVICES_INTERFACE_H_ +#define _KORTEX_{{package.short_name|upper}}_SERVICES_INTERFACE_H_ #include "ros/ros.h" @@ -25,9 +25,6 @@ #include #include -#include <{{package.short_name}}.pb.h> -#include <{{package.short_name}}ClientRpc.h> - {%- for method in package.methods %} #include "kortex_driver/{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}.h" {%- if method.is_notification_rpc %} @@ -42,27 +39,22 @@ using namespace std; -class {{package.short_name}}Services +class I{{package.short_name}}Services { public: - {{package.short_name}}Services(ros::NodeHandle& n, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms); + I{{package.short_name}}Services(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; {%- for method in package.methods %} - bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res); + virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) = 0; {%- if method.is_notification_rpc %} - void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif); + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) = 0; {%- endif %} {%- endfor %} -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - {{package.cpp_namespace}}::{{package.short_name}}Client* m_{{package.short_name|lower}}; - - ros::NodeHandle m_n; +protected: + ros::NodeHandle m_node_handle; ros::Publisher m_pub_Error; {%- for method in package.methods %} diff --git a/kortex_driver/templates/services.cpp.jinja2 b/kortex_driver/templates/services_robot.cpp.jinja2 similarity index 63% rename from kortex_driver/templates/services.cpp.jinja2 rename to kortex_driver/templates/services_robot.cpp.jinja2 index 36f5af45..d559de06 100644 --- a/kortex_driver/templates/services.cpp.jinja2 +++ b/kortex_driver/templates/services_robot.cpp.jinja2 @@ -19,36 +19,36 @@ {% endfor -%} #include "{{current_header_filename}}" -{{package.short_name}}Services::{{package.short_name}}Services(ros::NodeHandle& n, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +{{package.short_name}}RobotServices::{{package.short_name}}RobotServices(ros::NodeHandle& node_handle, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms): + I{{package.short_name}}Services(node_handle), m_{{package.short_name|lower}}({{package.short_name|lower}}), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); {%- for method in package.methods -%} {%- if method.is_notification_rpc %} - m_pub_{{method.name}} = m_n.advertise("{{method.name_lowercase_with_underscores}}", 1000); + m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); m_is_activated_{{method.name}} = false; {%- endif -%} {%- endfor %} - m_serviceSetDeviceID = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}Services::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}Services::SetApiOptions, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}RobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}RobotServices::SetApiOptions, this); {% for method in package.methods %} - m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_n.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); + m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}RobotServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); {%- endfor %} } -bool {{package.short_name}}Services::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool {{package.short_name}}RobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool {{package.short_name}}Services::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool {{package.short_name}}RobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -56,7 +56,7 @@ bool {{package.short_name}}Services::SetApiOptions(kortex_driver::SetApiOptions: } {% for method in package.methods %} -bool {{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) +bool {{package.short_name}}RobotServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) { {% if method.is_rpc_deprecated -%} ROS_WARN("The {{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}} service is now deprecated and will be removed in a future release."); @@ -82,7 +82,7 @@ bool {{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method {%- if not method.output_type_short_name == "Empty" %} {%- if not method.input_type_short_name == "Empty" %} {%- if method.is_notification_rpc %} - std::function< void ({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&{{package.short_name}}Services::cb_{{method.name}}, this, std::placeholders::_1); + std::function< void ({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&{{package.short_name}}RobotServices::cb_{{method.name}}, this, std::placeholders::_1); output = m_{{package.short_name|lower}}->{{method.prepend_on_notification}}{{method.name}}(callback, input, m_current_device_id); m_is_activated_{{method.name}} = true; {%- else %} @@ -123,7 +123,7 @@ bool {{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method return true; } {%- if method.is_notification_rpc %} -void {{package.short_name}}Services::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) +void {{package.short_name}}RobotServices::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) { kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; ToRosData(notif, ros_msg); diff --git a/kortex_driver/templates/services_robot.h.jinja2 b/kortex_driver/templates/services_robot.h.jinja2 new file mode 100644 index 00000000..7a19498d --- /dev/null +++ b/kortex_driver/templates/services_robot.h.jinja2 @@ -0,0 +1,48 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{package.short_name|upper}}_ROBOT_SERVICES_H_ +#define _KORTEX_{{package.short_name|upper}}_ROBOT_SERVICES_H_ + +#include "{{current_interface_header_filename}}" + +#include <{{package.short_name}}.pb.h> +#include <{{package.short_name}}ClientRpc.h> + +using namespace std; + +class {{package.short_name}}RobotServices : public I{{package.short_name}}Services +{ + public: + {{package.short_name}}RobotServices(ros::NodeHandle& node_handle, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; +{%- for method in package.methods %} + virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; +{%- if method.is_notification_rpc %} + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; +{%- endif %} +{%- endfor %} + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + {{package.cpp_namespace}}::{{package.short_name}}Client* m_{{package.short_name|lower}}; +}; +#endif + diff --git a/kortex_driver/templates/services_simulation.cpp.jinja2 b/kortex_driver/templates/services_simulation.cpp.jinja2 new file mode 100644 index 00000000..6c478764 --- /dev/null +++ b/kortex_driver/templates/services_simulation.cpp.jinja2 @@ -0,0 +1,81 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +{% for include_file_name in include_file_names -%} +#include "{{include_file_name}}" +{% endfor -%} +#include "{{current_header_filename}}" + +{{package.short_name}}SimulationServices::{{package.short_name}}SimulationServices(ros::NodeHandle& node_handle): + I{{package.short_name}}Services(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); +{%- for method in package.methods -%} +{%- if method.is_notification_rpc %} + m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); + m_is_activated_{{method.name}} = false; +{%- endif -%} +{%- endfor %} + + m_serviceSetDeviceID = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}SimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}SimulationServices::SetApiOptions, this); +{% for method in package.methods %} + m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}SimulationServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); +{%- endfor %} +} + +bool {{package.short_name}}SimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool {{package.short_name}}SimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +{% for method in package.methods %} +bool {{package.short_name}}SimulationServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) +{ + {% if method.is_rpc_deprecated -%} + ROS_WARN("The {{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}} service is now deprecated and will be removed in a future release."); + {% endif -%} + + {%- if method.is_notification_rpc %} + m_is_activated_{{method.name}} = true; + {%- endif %} + + if ({{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler) + { + res = {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for {{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}} is not implemented, so the service calls will return the default response."); + } + return true; +} +{%- if method.is_notification_rpc %} +void {{package.short_name}}SimulationServices::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) +{ + kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + ToRosData(notif, ros_msg); + m_pub_{{method.name}}.publish(ros_msg); +} +{%- endif %} +{% endfor -%} diff --git a/kortex_driver/templates/services_simulation.h.jinja2 b/kortex_driver/templates/services_simulation.h.jinja2 new file mode 100644 index 00000000..c2ccd970 --- /dev/null +++ b/kortex_driver/templates/services_simulation.h.jinja2 @@ -0,0 +1,41 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{package.short_name|upper}}_SIMULATION_SERVICES_H_ +#define _KORTEX_{{package.short_name|upper}}_SIMULATION_SERVICES_H_ + +#include "{{current_interface_header_filename}}" + +using namespace std; + +class {{package.short_name}}SimulationServices : public I{{package.short_name}}Services +{ + public: + {{package.short_name}}SimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; +{%- for method in package.methods %} + std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; + virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; +{%- if method.is_notification_rpc %} + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; +{%- endif %} +{%- endfor %} + +}; +#endif + diff --git a/kortex_examples/img/moveit.png b/kortex_examples/img/moveit.png new file mode 100644 index 00000000..f7572df5 Binary files /dev/null and b/kortex_examples/img/moveit.png differ diff --git a/kortex_examples/img/services_real_arm.png b/kortex_examples/img/services_real_arm.png new file mode 100644 index 00000000..7bf79abd Binary files /dev/null and b/kortex_examples/img/services_real_arm.png differ diff --git a/kortex_examples/img/services_sim.png b/kortex_examples/img/services_sim.png new file mode 100644 index 00000000..75eda69b Binary files /dev/null and b/kortex_examples/img/services_sim.png differ diff --git a/kortex_examples/package.xml b/kortex_examples/package.xml index ede7b258..6d9fbc58 100644 --- a/kortex_examples/package.xml +++ b/kortex_examples/package.xml @@ -1,10 +1,12 @@ kortex_examples - 2.2.0 - THe kortex package that act as a robot's driver. + 2.2.2 + Examples to show usage of core kortex_arm_driver services and topics. - + Hugo Lamontagne + Alexandre Vannobel + BSD diff --git a/kortex_examples/readme.md b/kortex_examples/readme.md index 08fa2943..ad9152cd 100644 --- a/kortex_examples/readme.md +++ b/kortex_examples/readme.md @@ -15,10 +15,11 @@ 1. [Before running an example](#first_of_all) -2. [Actuator configuration examples](#actuator_config) -3. [Full arm movement examples](#full_arm) -4. [Vision module configuration examples](#vision_config) -5. [MoveIt! examples](#move_it) +2. [Understanding the ways to use a Kortex arm with ROS](#the_ways) +3. [Actuator configuration examples](#actuator_config) +4. [Full arm movement examples](#full_arm) +5. [Vision module configuration examples](#vision_config) +6. [MoveIt! examples](#move_it) @@ -27,10 +28,56 @@ Before you run any example, make sure : - You have already built the packages using `catkin_make`. -- You are physically connected to an arm (or you are connected over Wi-Fi). -- You have started the `kortex_driver` node by following the [instructions](../kortex_driver/readme.md). +- You are physically connected to an arm (or you are connected over Wi-Fi) and you have started the `kortex_driver` node by following the [instructions](../kortex_driver/readme.md), or you have started the arm in simulation following the [instructions](../kortex_gazebo/readme.md). - The node started correctly and without errors. + +## The ways to use a Kortex arm with ROS + +There are a couple ways to use a Kortex arm with ROS, may it be in simulation or with a real arm. + +1. Using the auto-generated services and topics + + The driver auto-generates ROS services based on the C++ Kortex API, so every API call has its ROS equivalent. Some topics (not auto-generated) are also offered for convenience. You can read more about services, topics and notifications [here](../kortex_driver/readme.md#services). + + **With a real arm**, the auto-generated wrapper translates ROS requests to Kortex API requests (Protobuf), and translated responses back to ROS structures. + + ![](./img/services_real_arm.png) + + **In simulation**, the same services and topics are advertised by the kortex_driver node, but instead of translating to Kortex API and forwarding to an arm, the message either goes through on our own simulator (if the handler for the given service is implemented) or a default response is sent back and a warning printed. + + ![](./img/services_sim.png) + + Only a couple "core" services handlers have been implemented in simulation (mostly the ones that make the robot move and stop), namely: + - PlayJointTrajectory and the REACH_JOINT_ANGLES action type (to reach an angular goal) + - PlayCartesianTrajectory and the REACH_POSE action type (to reach a Cartesian goal) + - SendGripperCommand and the SEND_GRIPPER_COMMAND action type (to actuate the gripper) + - SendJointSpeedsCommand (for joint velocity control) + - ApplyEmergencyStop and Stop (to stop the robot) + - The Actions interface (ExecuteAction, ExecuteActionFromReference, CreateAction, DeleteAction, UpdateAction, StopAction). + + It is also important to note that IK solutions for real arms and simulated arms may vary, as KDL is used in simulation and our own kinematics library is used in the arm's firmware. + + The simulated SendTwistCommand service has a POC implementation, we decided it is not stable enough to be activated by default. You can uncomment the kortex_arm_driver.cpp simulation handler to re-enable it and try it yourself. The same goes for the Cartesian velocity topic. + + There is no plan to add more simulated services for now, but any user can write his own implementation of a Kortex ROS Service and enable the handler for it (let's say you want to simulate an Interconnect Expansion GPIO device, or a Vision device). The kortex_arm_driver.cpp file should provide all guidelines as to how to define your handler, and you are welcome to open an issue if you want more information on simulation handlers. + +2. Using MoveIt + + The kortex_driver offers a FollowJointTrajectory Action Server and a GripperCommand Action Server (when a gripper is used) and the MoveIt configuration files are stored for all configurations in kortex_move_it_config. This enables users to use the MoveIt Commander, or the MoveIt Python or C++ interfaces to control the arm with the motion planning framework. + + ![](./img/moveit.png) + + **With a real arm**, the FollowJointTrajectory Action Server uses the Kortex API `PlayPreComputedJointTrajectory`. This call expects a full joint trajectory interpolated at a 1ms timestep, because the arm takes the trajectory as is, verifies it respects all velocity and acceleration constraints and then executes it as a low-level trajectory. Because of this timestep constraint, the MoveIt OMPL planning pipeline has an additional step which uses the `industrial_trajectory_filters/UniformSampleFilter` to interpolate the MoveIt output with a 1ms-timestep. Any timestep that provides a wrong velocity or acceleration causes the whole trajectory to be rejected. The velocity and acceleration limits in the configuration files have been tuned so no trajectory should yield such values, but if you experience trajectory rejection problems, you can tune down those parameters. + + **In simulation**, the FollowJointTrajectory and GripperCommand Action Servers are the ones spawned by the ros_controllers used with Gazebo. They don't need 1ms-timestep, but no option was added in MoveIt to switch between simulated or real action servers, so the same interpolator is used in simulation although it is not required. The Gen3 Intel Realsense camera is not simulated. + +3. Low-level control + + **With a real arm**, the low-level control functions have not been added to the Kortex API wrapper because the arm absolutely needs 1kHz control, otherwise it jerks. As ROS is not really real-time friendly, we chose not to offer those functions. + + **In simulation**, the ros_controllers used with Gazebo can be directly controlled with their associated topics if you prefer controlling the joints directly without using the simulation handlers, but be aware that this interface is not accessible with the real arm! The code you write that way will need to be changed significantly to be used with a real arm. + ## Actuator configuration examples @@ -38,13 +85,13 @@ Before you run any example, make sure : The examples look for advertised services in the **my_gen3** namespace by default and configures the first actuator. -To run the C++ example: `rosrun kortex_examples example_actuator_configuration_cpp` +To run the C++ example: `roslaunch kortex_examples actuator_config_cpp.launch` -To run the Python example: `rosrun kortex_examples example_actuator_configuration.py` +To run the Python example: `roslaunch kortex_examples actuator_config_python.launch` If you started the `kortex_driver` node in another namespace (not **my_gen3**) or if you want to test the example on another actuator than the first one, you will have to supply node parameters in the command line (the syntax doesn't change if you run the C++ or Python example): -`rosrun kortex_examples example_actuator_configuration_cpp _robot_name:= _device_id:=` +`roslaunch kortex_examples actuator_config_cpp.launch robot_name:= device_id:=` ## Full arm examples @@ -54,21 +101,21 @@ The examples look for advertised services in the **my_gen3** namespace by defaul - **Simple movement example**: - - To run the C++ example: `rosrun kortex_examples example_full_arm_movement_cpp` - - To run the Python example: `rosrun kortex_examples example_full_arm_movement.py` + - To run the C++ example: `roslaunch kortex_examples full_arm_movement_cpp.launch` + - To run the Python example: `roslaunch kortex_examples full_arm_movement_python.launch` If you started the `kortex_driver` node in another namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : - `rosrun kortex_examples example_full_arm_movement_cpp _robot_name:=` + `roslaunch kortex_examples full_arm_movement_cpp.launch robot_name:=` - **Cartesian poses with notifications**: - - To run the C++ example: `rosrun kortex_examples example_cartesian_poses_with_notifications_cpp` - - To run the Python example: `rosrun kortex_examples example_cartesian_poses_with_notifications.py` + - To run the C++ example: `roslaunch kortex_examples cartesian_poses_with_notifications_cpp.launch` + - To run the Python example: `roslaunch kortex_examples cartesian_poses_with_notifications_python.launch` If you started the `kortex_driver` node in another namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : - `rosrun kortex_examples example_cartesian_poses_with_notifications_cpp _robot_name:=` + `roslaunch kortex_examples cartesian_poses_with_notifications_cpp.launch robot_name:=` ## Vision module configuration examples @@ -76,13 +123,13 @@ The examples look for advertised services in the **my_gen3** namespace by defaul The examples look for advertised services in the **my_gen3** namespace by default. -To run the C++ example: `rosrun kortex_examples example_vision_configuration_cpp` +To run the C++ example: `roslaunch kortex_examples vision_configuration_cpp.launch` -To run the Python example: `rosrun kortex_examples example_vision_configuration.py` +To run the Python example: `roslaunch kortex_examples vision_configuration_python.launch` If you started the `kortex_driver` node in another namespace (not **my_gen3**), you will have to supply the node a parameter in the command line (the syntax doesn't change if you run the C++ or Python example) : -`rosrun kortex_examples example_vision_configuration_cpp _robot_name:=` +`roslaunch kortex_examples vision_configuration_cpp.launch robot_name:=` ## MoveIt! example diff --git a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp index 53baf774..d79d87a8 100644 --- a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp +++ b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp @@ -25,65 +25,40 @@ #include #include #include +#include #include #include #define HOME_ACTION_IDENTIFIER 2 -bool Pose1Done = false; -bool Pose2Done = false; -bool Pose3Done = false; - bool all_notifs_succeeded = true; -//Callback that is called when the robot publish a ActionNotification -void notification_callback(const kortex_driver::ActionNotification::ConstPtr& notif) +std::atomic last_action_notification_event{0}; +std::atomic last_action_notification_id{0}; + +void notification_callback(const kortex_driver::ActionNotification& notif) +{ + last_action_notification_event = notif.action_event; + last_action_notification_id = notif.handle.identifier; +} + +bool wait_for_action_end_or_abort() { - switch(notif->action_event) + while (ros::ok()) { - //This event is published when an action has ended or was aborted - case kortex_driver::ActionEvent::ACTION_END: + if (last_action_notification_event.load() == kortex_driver::ActionEvent::ACTION_END) { - if(notif->handle.identifier == 1001) - { - Pose1Done = true; - } - - if(notif->handle.identifier == 1002) - { - Pose2Done = true; - } - - if(notif->handle.identifier == 1003) - { - Pose3Done = true; - } - break; + ROS_INFO("Received ACTION_END notification for action %d", last_action_notification_id.load()); + return true; } - case kortex_driver::ActionEvent::ACTION_ABORT: + else if (last_action_notification_event.load() == kortex_driver::ActionEvent::ACTION_ABORT) { - ROS_ERROR("Action was aborted!"); + ROS_INFO("Received ACTION_ABORT notification for action %d", last_action_notification_id.load()); all_notifs_succeeded = false; - if(notif->handle.identifier == 1001) - { - Pose1Done = true; - } - - if(notif->handle.identifier == 1002) - { - Pose2Done = true; - } - - if(notif->handle.identifier == 1003) - { - Pose3Done = true; - } - break; + return false; } - default: - break; + ros::spinOnce(); } - } bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) @@ -118,7 +93,7 @@ bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) return false; } - return true; + return wait_for_action_end_or_abort(); } bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) @@ -126,7 +101,7 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) ros::ServiceClient service_client_activate_notif = n.serviceClient("/" + robot_name + "/base/activate_publishing_of_action_topic"); kortex_driver::OnNotificationActionTopic service_activate_notif; - //We need to call this service to activate the Action Notification on the kortex_driver node. + // We need to call this service to activate the Action Notification on the kortex_driver node. if (service_client_activate_notif.call(service_activate_notif)) { ROS_INFO("Action notification activated!"); @@ -161,7 +136,9 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) service_execute_action.request.input.oneof_action_parameters.reach_pose.push_back(my_constrained_pose); service_execute_action.request.input.name = "pose1"; service_execute_action.request.input.handle.identifier = 1001; + service_execute_action.request.input.handle.action_type = kortex_driver::ActionType::REACH_POSE; + last_action_notification_event = 0; if (service_client_execute_action.call(service_execute_action)) { ROS_INFO("Pose 1 was sent to the robot."); @@ -174,10 +151,7 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) } // Waiting for the pose 1 to end - while(!Pose1Done) - { - ros::spinOnce(); - } + wait_for_action_end_or_abort(); // Change the pose and give it a new identifier service_execute_action.request.input.handle.identifier = 1002; @@ -188,6 +162,7 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) service_execute_action.request.input.oneof_action_parameters.reach_pose.clear(); service_execute_action.request.input.oneof_action_parameters.reach_pose.push_back(my_constrained_pose); + last_action_notification_event = 0; if (service_client_execute_action.call(service_execute_action)) { ROS_INFO("Pose 2 was sent to the robot."); @@ -199,11 +174,8 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) return false; } - //Waiting for the pose 2 to end. - while(!Pose2Done) - { - ros::spinOnce(); - } + // Waiting for the pose 2 to end. + wait_for_action_end_or_abort(); // Change the pose and give it a new identifier service_execute_action.request.input.handle.identifier = 1003; @@ -214,6 +186,7 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) service_execute_action.request.input.oneof_action_parameters.reach_pose.clear(); service_execute_action.request.input.oneof_action_parameters.reach_pose.push_back(my_constrained_pose); + last_action_notification_event = 0; if (service_client_execute_action.call(service_execute_action)) { ROS_INFO("Pose 3 was sent to the robot."); @@ -226,10 +199,7 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) } // Waiting for the pose 3 to end - while(!Pose3Done) - { - ros::spinOnce(); - } + wait_for_action_end_or_abort(); return true; } @@ -289,7 +259,6 @@ int main(int argc, char **argv) success &= example_set_cartesian_reference_frame(n, robot_name); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); success &= example_home_the_robot(n, robot_name); - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); success &= example_cartesian_action(n, robot_name); success &= all_notifs_succeeded; @@ -297,6 +266,4 @@ int main(int argc, char **argv) ros::param::set("/kortex_examples_test_results/cartesian_poses_with_notifications_cpp", success); return success ? 0 : 1; - - return 0; } \ No newline at end of file diff --git a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py index d5af91b4..2e5abbf3 100755 --- a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py +++ b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py @@ -13,7 +13,7 @@ import sys import rospy -import threading +import time from kortex_driver.srv import * from kortex_driver.msg import * @@ -24,10 +24,7 @@ def __init__(self): self.HOME_ACTION_IDENTIFIER = 2 - self.pose_1_done = threading.Event() - self.pose_2_done = threading.Event() - self.pose_3_done = threading.Event() - + self.action_topic_sub = None self.all_notifs_succeeded = True # Get node params @@ -35,6 +32,10 @@ def __init__(self): rospy.loginfo("Using robot_name " + self.robot_name) + # Init the action topic subscriber + self.action_topic_sub = rospy.Subscriber("/" + self.robot_name + "/action_topic", ActionNotification, self.cb_action_topic) + self.last_action_notif_type = None + # Init the services clear_faults_full_name = '/' + self.robot_name + '/base/clear_faults' rospy.wait_for_service(clear_faults_full_name) @@ -60,6 +61,21 @@ def __init__(self): else: self.is_init_success = True + def cb_action_topic(self, notif): + self.last_action_notif_type = notif.action_event + + def wait_for_action_end_or_abort(self): + while not rospy.is_shutdown(): + if (self.last_action_notif_type == ActionEvent.ACTION_END): + rospy.loginfo("Received ACTION_END notification") + return True + elif (self.last_action_notif_type == ActionEvent.ACTION_ABORT): + rospy.loginfo("Received ACTION_ABORT notification") + self.all_notifs_succeeded = False + return False + else: + time.sleep(0.01) + def example_clear_faults(self): try: self.clear_faults() @@ -75,6 +91,7 @@ def example_home_the_robot(self): # The Home Action is used to home the robot. It cannot be deleted and is always ID #2: req = ReadActionRequest() req.input.identifier = self.HOME_ACTION_IDENTIFIER + self.last_action_notif_type = None try: res = self.read_action(req) except rospy.ServiceException: @@ -92,7 +109,7 @@ def example_home_the_robot(self): rospy.logerr("Failed to call ExecuteAction") return False else: - return True + return self.wait_for_action_end_or_abort() def example_set_cartesian_reference_frame(self): # Prepare the request with the frame we want to set @@ -112,24 +129,6 @@ def example_set_cartesian_reference_frame(self): # Wait a bit rospy.sleep(0.25) - def notification_callback(self, notif): - if notif.action_event == ActionEvent.ACTION_END: - if notif.handle.identifier == 1001: - self.pose_1_done.set() - if notif.handle.identifier == 1002: - self.pose_2_done.set() - if notif.handle.identifier == 1003: - self.pose_3_done.set() - if notif.action_event == ActionEvent.ACTION_ABORT: - rospy.logerr("Action was aborted!") - self.all_notifs_succeeded = False - if notif.handle.identifier == 1001: - self.pose_1_done.set() - if notif.handle.identifier == 1002: - self.pose_2_done.set() - if notif.handle.identifier == 1003: - self.pose_3_done.set() - def example_subscribe_to_a_robot_notification(self): # Activate the publishing of the ActionNotification req = OnNotificationActionTopicRequest() @@ -142,9 +141,6 @@ def example_subscribe_to_a_robot_notification(self): else: rospy.loginfo("Successfully activated the Action Notifications!") - # Subscribe to the ActionNotification with the given callback - rospy.Subscriber("/" + self.robot_name + "/action_topic", ActionNotification, self.notification_callback) - rospy.sleep(1.0) return True @@ -167,7 +163,6 @@ def main(self): #******************************************************************************* # Start the example from the Home position success &= self.example_home_the_robot() - rospy.sleep(7) #******************************************************************************* #******************************************************************************* @@ -197,9 +192,11 @@ def main(self): req = ExecuteActionRequest() req.input.oneof_action_parameters.reach_pose.append(my_constrained_pose) req.input.name = "pose1" + req.input.handle.action_type = ActionType.REACH_POSE req.input.handle.identifier = 1001 rospy.loginfo("Sending pose 1...") + self.last_action_notif_type = None try: self.execute_action(req) except rospy.ServiceException: @@ -208,7 +205,7 @@ def main(self): else: rospy.loginfo("Waiting for pose 1 to finish...") - self.pose_1_done.wait() + self.wait_for_action_end_or_abort() # Prepare and send pose 2 req.input.handle.identifier = 1002 @@ -219,6 +216,7 @@ def main(self): req.input.oneof_action_parameters.reach_pose[0] = my_constrained_pose rospy.loginfo("Sending pose 2...") + self.last_action_notif_type = None try: self.execute_action(req) except rospy.ServiceException: @@ -227,7 +225,7 @@ def main(self): else: rospy.loginfo("Waiting for pose 2 to finish...") - self.pose_2_done.wait() + self.wait_for_action_end_or_abort() # Prepare and send pose 3 req.input.handle.identifier = 1003 @@ -238,6 +236,7 @@ def main(self): req.input.oneof_action_parameters.reach_pose[0] = my_constrained_pose rospy.loginfo("Sending pose 3...") + self.last_action_notif_type = None try: self.execute_action(req) except rospy.ServiceException: @@ -246,7 +245,7 @@ def main(self): else: rospy.loginfo("Waiting for pose 3 to finish...") - self.pose_3_done.wait() + self.wait_for_action_end_or_abort() success &= self.all_notifs_succeeded diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.cpp b/kortex_examples/src/full_arm/example_full_arm_movement.cpp index 2ed97ee0..8459f123 100644 --- a/kortex_examples/src/full_arm/example_full_arm_movement.cpp +++ b/kortex_examples/src/full_arm/example_full_arm_movement.cpp @@ -10,6 +10,7 @@ * */ #include +#include #include "ros/ros.h" #include @@ -23,9 +24,39 @@ #include #include #include +#include +#include +#include #define HOME_ACTION_IDENTIFIER 2 +std::atomic last_action_notification_event{0}; + +void notification_callback(const kortex_driver::ActionNotification& notif) +{ + last_action_notification_event = notif.action_event; +} + +bool wait_for_action_end_or_abort() +{ + while (ros::ok()) + { + if (last_action_notification_event.load() == kortex_driver::ActionEvent::ACTION_END) + { + ROS_INFO("Received ACTION_END notification"); + return true; + } + else if (last_action_notification_event.load() == kortex_driver::ActionEvent::ACTION_ABORT) + { + ROS_INFO("Received ACTION_ABORT notification"); + return false; + } + ros::spinOnce(); + } +} + + + bool example_clear_faults(ros::NodeHandle n, std::string robot_name) { ros::ServiceClient service_client_clear_faults = n.serviceClient("/" + robot_name + "/base/clear_faults"); @@ -48,6 +79,7 @@ bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) { ros::ServiceClient service_client_read_action = n.serviceClient("/" + robot_name + "/base/read_action"); kortex_driver::ReadAction service_read_action; + last_action_notification_event = 0; // The Home Action is used to home the robot. It cannot be deleted and is always ID #2: service_read_action.request.input.identifier = HOME_ACTION_IDENTIFIER; @@ -76,7 +108,7 @@ bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) return false; } - return true; + return wait_for_action_end_or_abort(); } bool example_set_cartesian_reference_frame(ros::NodeHandle n, std::string robot_name) @@ -101,6 +133,7 @@ bool example_set_cartesian_reference_frame(ros::NodeHandle n, std::string robot_ bool example_send_cartesian_pose(ros::NodeHandle n, std::string robot_name) { + last_action_notification_event = 0; // Get the actual cartesian pose to increment it // You can create a subscriber to listen to the base_feedback // Here we only need the latest message in the topic though @@ -121,10 +154,10 @@ bool example_send_cartesian_pose(ros::NodeHandle n, std::string robot_name) // Creating the target pose service_play_cartesian_trajectory.request.input.target_pose.x = current_x; service_play_cartesian_trajectory.request.input.target_pose.y = current_y; - service_play_cartesian_trajectory.request.input.target_pose.z = current_z + 0.15; + service_play_cartesian_trajectory.request.input.target_pose.z = current_z + 0.10; service_play_cartesian_trajectory.request.input.target_pose.theta_x = current_theta_x; service_play_cartesian_trajectory.request.input.target_pose.theta_y = current_theta_y; - service_play_cartesian_trajectory.request.input.target_pose.theta_z = current_theta_z + 35; + service_play_cartesian_trajectory.request.input.target_pose.theta_z = current_theta_z; kortex_driver::CartesianSpeed poseSpeed; poseSpeed.translation = 0.1; @@ -145,11 +178,12 @@ bool example_send_cartesian_pose(ros::NodeHandle n, std::string robot_name) return false; } - return true; + return wait_for_action_end_or_abort(); } bool example_send_joint_angles(ros::NodeHandle n, std::string robot_name, int degrees_of_freedom) { + last_action_notification_event = 0; // Initialize the ServiceClient ros::ServiceClient service_client_play_joint_trajectory = n.serviceClient("/" + robot_name + "/base/play_joint_trajectory"); kortex_driver::PlayJointTrajectory service_play_joint_trajectory; @@ -179,7 +213,7 @@ bool example_send_joint_angles(ros::NodeHandle n, std::string robot_name, int de return false; } - return true; + return wait_for_action_end_or_abort(); } bool example_send_gripper_command(ros::NodeHandle n, std::string robot_name, double value) @@ -205,7 +239,7 @@ bool example_send_gripper_command(ros::NodeHandle n, std::string robot_name, dou ROS_ERROR("%s", error_string.c_str()); return false; } - + std::this_thread::sleep_for(std::chrono::milliseconds(500)); return true; } @@ -262,6 +296,23 @@ int main(int argc, char **argv) } //******************************************************************************* + // Subscribe to the Action Topic + ros::Subscriber sub = n.subscribe("/" + robot_name + "/action_topic", 1000, notification_callback); + + // We need to call this service to activate the Action Notification on the kortex_driver node. + ros::ServiceClient service_client_activate_notif = n.serviceClient("/" + robot_name + "/base/activate_publishing_of_action_topic"); + kortex_driver::OnNotificationActionTopic service_activate_notif; + if (service_client_activate_notif.call(service_activate_notif)) + { + ROS_INFO("Action notification activated!"); + } + else + { + std::string error_string = "Action notification publication failed"; + ROS_ERROR("%s", error_string.c_str()); + success = false; + } + //******************************************************************************* // Make sure to clear the robot's faults else it won't move if it's already in fault success &= example_clear_faults(n, robot_name); @@ -270,7 +321,6 @@ int main(int argc, char **argv) //******************************************************************************* // Move the robot to the Home position with an Action success &= example_home_the_robot(n, robot_name); - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); //******************************************************************************* //******************************************************************************* @@ -279,7 +329,6 @@ int main(int argc, char **argv) if (is_gripper_present) { success &= example_send_gripper_command(n, robot_name, 0.0); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } //******************************************************************************* @@ -290,14 +339,12 @@ int main(int argc, char **argv) // Example of cartesian pose // Let's make it move in Z success &= example_send_cartesian_pose(n, robot_name); - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); //******************************************************************************* //******************************************************************************* // Example of angular position // Let's send the arm to vertical position success &= example_send_joint_angles(n, robot_name, degrees_of_freedom); - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); //******************************************************************************* //******************************************************************************* @@ -306,7 +353,6 @@ int main(int argc, char **argv) if (is_gripper_present) { success &= example_send_gripper_command(n, robot_name, 0.5); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } //******************************************************************************* diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.py b/kortex_examples/src/full_arm/example_full_arm_movement.py index 81449913..14868b1f 100755 --- a/kortex_examples/src/full_arm/example_full_arm_movement.py +++ b/kortex_examples/src/full_arm/example_full_arm_movement.py @@ -13,6 +13,7 @@ import sys import rospy +import time from kortex_driver.srv import * from kortex_driver.msg import * @@ -30,6 +31,10 @@ def __init__(self): rospy.loginfo("Using robot_name " + self.robot_name + " , robot has " + str(self.degrees_of_freedom) + " degrees of freedom and is_gripper_present is " + str(self.is_gripper_present)) + # Init the action topic subscriber + self.action_topic_sub = rospy.Subscriber("/" + self.robot_name + "/action_topic", ActionNotification, self.cb_action_topic) + self.last_action_notif_type = None + # Init the services clear_faults_full_name = '/' + self.robot_name + '/base/clear_faults' rospy.wait_for_service(clear_faults_full_name) @@ -58,11 +63,44 @@ def __init__(self): send_gripper_command_full_name = '/' + self.robot_name + '/base/send_gripper_command' rospy.wait_for_service(send_gripper_command_full_name) self.send_gripper_command = rospy.ServiceProxy(send_gripper_command_full_name, SendGripperCommand) + + activate_publishing_of_action_notification_full_name = '/' + self.robot_name + '/base/activate_publishing_of_action_topic' + rospy.wait_for_service(activate_publishing_of_action_notification_full_name) + self.activate_publishing_of_action_notification = rospy.ServiceProxy(activate_publishing_of_action_notification_full_name, OnNotificationActionTopic) except: self.is_init_success = False else: self.is_init_success = True + def cb_action_topic(self, notif): + self.last_action_notif_type = notif.action_event + + def wait_for_action_end_or_abort(self): + while not rospy.is_shutdown(): + if (self.last_action_notif_type == ActionEvent.ACTION_END): + rospy.loginfo("Received ACTION_END notification") + return True + elif (self.last_action_notif_type == ActionEvent.ACTION_ABORT): + rospy.loginfo("Received ACTION_ABORT notification") + return False + else: + time.sleep(0.01) + + def example_subscribe_to_a_robot_notification(self): + # Activate the publishing of the ActionNotification + req = OnNotificationActionTopicRequest() + rospy.loginfo("Activating the action notifications...") + try: + self.activate_publishing_of_action_notification(req) + except rospy.ServiceException: + rospy.logerr("Failed to call OnNotificationActionTopic") + return False + else: + rospy.loginfo("Successfully activated the Action Notifications!") + + rospy.sleep(1.0) + return True + def example_clear_faults(self): try: self.clear_faults() @@ -76,6 +114,7 @@ def example_clear_faults(self): def example_home_the_robot(self): # The Home Action is used to home the robot. It cannot be deleted and is always ID #2: + self.last_action_notif_type = None req = ReadActionRequest() req.input.identifier = self.HOME_ACTION_IDENTIFIER try: @@ -95,9 +134,10 @@ def example_home_the_robot(self): rospy.logerr("Failed to call ExecuteAction") return False else: - return True + return self.wait_for_action_end_or_abort() def example_set_cartesian_reference_frame(self): + self.last_action_notif_type = None # Prepare the request with the frame we want to set req = SetCartesianReferenceFrameRequest() req.input.reference_frame = CartesianReferenceFrame.CARTESIAN_REFERENCE_FRAME_MIXED @@ -116,6 +156,7 @@ def example_set_cartesian_reference_frame(self): return True def example_send_cartesian_pose(self): + self.last_action_notif_type = None # Get the actual cartesian pose to increment it # You can create a subscriber to listen to the base_feedback # Here we only need the latest message in the topic though @@ -124,10 +165,10 @@ def example_send_cartesian_pose(self): req = PlayCartesianTrajectoryRequest() req.input.target_pose.x = feedback.base.commanded_tool_pose_x req.input.target_pose.y = feedback.base.commanded_tool_pose_y - req.input.target_pose.z = feedback.base.commanded_tool_pose_z + 0.15 + req.input.target_pose.z = feedback.base.commanded_tool_pose_z + 0.10 req.input.target_pose.theta_x = feedback.base.commanded_tool_pose_theta_x req.input.target_pose.theta_y = feedback.base.commanded_tool_pose_theta_y - req.input.target_pose.theta_z = feedback.base.commanded_tool_pose_theta_z + 35 + req.input.target_pose.theta_z = feedback.base.commanded_tool_pose_theta_z pose_speed = CartesianSpeed() pose_speed.translation = 0.1 @@ -145,9 +186,10 @@ def example_send_cartesian_pose(self): rospy.logerr("Failed to call PlayCartesianTrajectory") return False else: - return True + return self.wait_for_action_end_or_abort() def example_send_joint_angles(self): + self.last_action_notif_type = None # Create the list of angles req = PlayJointTrajectoryRequest() # Here the arm is vertical (all zeros) @@ -165,11 +207,10 @@ def example_send_joint_angles(self): rospy.logerr("Failed to call PlayJointTrajectory") return False else: - return True + return self.wait_for_action_end_or_abort() def example_send_gripper_command(self, value): # Initialize the request - # This works for the Robotiq Gripper 2F_85 # Close the gripper req = SendGripperCommandRequest() finger = Finger() @@ -187,6 +228,7 @@ def example_send_gripper_command(self, value): rospy.logerr("Failed to call SendGripperCommand") return False else: + time.sleep(0.5) return True def main(self): @@ -202,11 +244,15 @@ def main(self): # Make sure to clear the robot's faults else it won't move if it's already in fault success &= self.example_clear_faults() #******************************************************************************* + + #******************************************************************************* + # Activate the action notifications + success &= self.example_subscribe_to_a_robot_notification() + #******************************************************************************* #******************************************************************************* # Move the robot to the Home position with an Action success &= self.example_home_the_robot() - rospy.sleep(10.0) #******************************************************************************* #******************************************************************************* @@ -214,9 +260,8 @@ def main(self): # Let's fully open the gripper if self.is_gripper_present: success &= self.example_send_gripper_command(0.0) - rospy.sleep(2.0) else: - rospy.logwarn("No gripper is present on the arm.") + rospy.logwarn("No gripper is present on the arm.") #******************************************************************************* #******************************************************************************* @@ -226,14 +271,12 @@ def main(self): # Example of cartesian pose # Let's make it move in Z success &= self.example_send_cartesian_pose() - rospy.sleep(10.0) #******************************************************************************* #******************************************************************************* # Example of angular position # Let's send the arm to vertical position success &= self.example_send_joint_angles() - rospy.sleep(10.0) #******************************************************************************* #******************************************************************************* @@ -241,7 +284,6 @@ def main(self): # Let's close the gripper at 50% if self.is_gripper_present: success &= self.example_send_gripper_command(0.5) - rospy.sleep(2.0) else: rospy.logwarn("No gripper is present on the arm.") #******************************************************************************* diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 646f42c8..bc0f550a 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -11,21 +11,24 @@ - + - + + + + + - - - + + - + @@ -42,10 +45,10 @@ - - @@ -55,98 +58,109 @@ - - + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_gazebo/package.xml b/kortex_gazebo/package.xml index 100ca5da..850ac4da 100644 --- a/kortex_gazebo/package.xml +++ b/kortex_gazebo/package.xml @@ -1,15 +1,16 @@ kortex_gazebo - 2.2.0 + 2.2.2

Gazebo simulation package for Kortex robots

-

This package contains launch files to spawn the Kortex robot in Gazebo and visualize it in RViz

+

This package contains launch files to spawn the Kortex robot in Gazebo and visualize it in RViz, as well as configuration files.

 Alexandre Vannobel - + BSD catkin roslaunch + kortex_driver robot_state_publisher rviz joint_state_publisher diff --git a/kortex_gazebo/readme.md b/kortex_gazebo/readme.md index 206a55b9..ca3c3391 100644 --- a/kortex_gazebo/readme.md +++ b/kortex_gazebo/readme.md @@ -10,7 +10,7 @@ * * --> -# Kortex Gazebo (**EXPERIMENTAL**) +# Kortex Gazebo ## Overview This package contains files to simulate the Kinova Gen3 and Gen3 lite robots in Gazebo. @@ -25,18 +25,18 @@ Maintainer: Kinova inc. support@kinovarobotics.com** This package has been tested under ROS Kinetic, Gazebo 7 and Ubuntu 16.04 and under ROS Melodic, Gazebo 9 and Ubuntu 18.04. -## Why **"EXPERIMENTAL"**? +## A word on stability of the models -The simulated arm has been tested and is stable in mid and high-inertia configurations and in most of its workspace, but the PID gains specified in the [configuration file](../kortex_control/arms/gen3/config/joint_position_controllers.yaml) do not work well in very low-inertia configurations (when the arm is vertical, or almost vertical). Hence, since some tuning still needs to be done on the gains, the package is tagged as **experimental**. +The arms's controllers are simulated with the `gazebo_ros_control` package, which provides PID effort controllers for every joint. It is not an exact simulation of the real arms's behaviour. The simulated arms are stable in most of their workspace, although we have seen the Gen3 be a bit less stable in a very low inertia position (vertical). The gains are set in .yaml files in the `kortex_control` package, so they can be modified by end users. Other disclaimers : - The simulation has not been tested with simulated payload. - - No characterization was done to compare and quantify the performance of the simulated controllers and the real arm's controllers. The simulation only offers a **visually comparable experience** to the real arm. + - No characterization was done to compare and quantify the performance of the simulated controllers and the real arm's controllers. The simulation only offers a **visually comparable experience** to the real arms. - Very rarely, the base (fixed to the world frame) seems to lose its fixation and the arm goes unstable. We are currently looking for the root cause of this behavior, but we found that clicking **Edit ---> Reset Model Poses** (Shift + Ctrl + R) in Gazebo is a workaround. ## Usage -The [spawn_kortex_robot.launch file](launch/spawn_kortex_robot.launch) launches the arm simulation in [Gazebo](http://gazebosim.org), with [ros_control](http://wiki.ros.org/ros_control) controllers and optionally [MoveIt!](https://moveit.ros.org/). +The [spawn_kortex_robot.launch file](launch/spawn_kortex_robot.launch) launches the arm simulation in [Gazebo](http://gazebosim.org), with [ros_control](http://wiki.ros.org/ros_control) controllers and [MoveIt!](https://moveit.ros.org/). The launch can be parametrized with arguments : **Arguments**: @@ -70,24 +70,19 @@ Gazebo loads an empty world an first, then the `spawn_model` node from the [gaze ## Controllers -### JointTrajectoryController with MoveIt! support - The simulated arm is controlled with a `effort_controllers/JointTrajectoryController` from [ros_controllers](http://wiki.ros.org/ros_controllers). This controller offers a [FollowJointTrajectory](http://wiki.ros.org/joint_trajectory_controller) interface to control the simulated arm, which is configured with [MoveIt!](http://docs.ros.org/kinetic/api/moveit_tutorials/html/index.html) so the trajectories outputed by the `move_group` node are sent to the robot in Gazebo and executed. -The RViz Motion Planning plugin offers simple motion planning support for the simulated robot. See the [kortex_move_it_config documentation](../kortex_move_it_config/readme.md) for more details. - -See the [kortex_control package documentation](../kortex_control/readme.md) for more details. +The RViz Motion Planning plugin offers simple motion planning support for the simulated robot. See the [kortex_move_it_config documentation](../kortex_move_it_config/readme.md) for more details. -### Individual JointPositionController's for each joint +There are also individual JointPositionController's for every joint, stopped by default. These are used by the kortex_arm_driver node to do velocity control. -The simulated arm is controlled with one `effort_controllers/JointPositionController` per joint. -These controllers offer a simpler topic interface to control the simulated arm joint by joint. These controllers cannot be used with MoveIt!, which needs a `FollowJointTrajectory` interface. +See the [kortex_driver package documentation](../kortex_driver/readme.md) for more details on how to use the simulated arm. ## Initialization script The package also uses a [Python script](./scripts/home_robot.py) to home the robot after the robot has been spawned. -Gazebo is **launched with paused physics** when using the trajectory controller. Otherwise, the arm would fall to the ground because the controllers are not fully loaded when the robot is spawned in the simulator. -When everything is well loaded, the script unpauses Gazebo's physics and uses MoveIt! to home the robot. +Gazebo is **launched with paused physics**. Otherwise, the arm would fall to the ground because the controllers are not fully loaded when the robot is spawned in the simulator. +When everything is well loaded, the script unpauses Gazebo's physics and executes the robot's Home action. ## Plugins diff --git a/kortex_gazebo/scripts/home_robot.py b/kortex_gazebo/scripts/home_robot.py index c95c0a92..26ef88f8 100755 --- a/kortex_gazebo/scripts/home_robot.py +++ b/kortex_gazebo/scripts/home_robot.py @@ -1,74 +1,79 @@ #!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, SRI International -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of SRI International nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Acorn Pooley, Mike Lautman - -# Inspired from http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html -# Modified by Alexandre Vannobel to initialize simulated Kinova Gen3 robot in Gazebo - import sys import time import rospy -import moveit_commander -import moveit_msgs.msg -import geometry_msgs.msg from std_srvs.srv import Empty +from kortex_driver.msg import ActionNotification, ActionEvent +from kortex_driver.srv import ReadAction, ReadActionRequest, ExecuteAction, ExecuteActionRequest class ExampleInitializeGazeboRobot(object): - """home_robot""" + """Unpause Gazebo and home robot""" + def __init__(self): # Initialize the node - super(ExampleInitializeGazeboRobot, self).__init__() - rospy.init_node('init_robot') + self.HOME_ACTION_IDENTIFIER = 2 + self.last_action_notif_type = None try: - # Create the MoveItInterface necessary objects - moveit_commander.roscpp_initialize(sys.argv) - group_name = "arm" - self.robot = moveit_commander.RobotCommander() - self.scene = moveit_commander.PlanningSceneInterface() - self.group = moveit_commander.MoveGroupCommander(group_name) - except Exception as e: - rospy.logerr(e) + self.robot_name = rospy.get_param('~robot_name') + self.action_topic_sub = rospy.Subscriber("/" + self.robot_name + "/action_topic", ActionNotification, self.cb_action_topic) + + # Wait for the driver to be initialised + while not rospy.has_param("/" + self.robot_name + "/is_initialized"): + time.sleep(0.1) + + # Init the services + read_action_full_name = '/' + self.robot_name + '/base/read_action' + rospy.wait_for_service(read_action_full_name) + self.read_action = rospy.ServiceProxy(read_action_full_name, ReadAction) + + execute_action_full_name = '/' + self.robot_name + '/base/execute_action' + rospy.wait_for_service(execute_action_full_name) + self.execute_action = rospy.ServiceProxy(execute_action_full_name, ExecuteAction) + except rospy.ROSException as e: self.is_init_success = False else: self.is_init_success = True + def cb_action_topic(self, notif): + self.last_action_notif_type = notif.action_event + + def wait_for_action_end_or_abort(self): + while not rospy.is_shutdown(): + if (self.last_action_notif_type == ActionEvent.ACTION_END): + rospy.loginfo("Received ACTION_END notification") + return True + elif (self.last_action_notif_type == ActionEvent.ACTION_ABORT): + rospy.loginfo("Received ACTION_ABORT notification") + return False + else: + time.sleep(0.01) + def home_the_robot(self): - # Home the robot - self.group.set_named_target("home") - return self.group.go(wait=True) + # The Home Action is used to home the robot. It cannot be deleted and is always ID #2: + self.last_action_notif_type = None + req = ReadActionRequest() + req.input.identifier = self.HOME_ACTION_IDENTIFIER + + try: + res = self.read_action(req) + except rospy.ServiceException: + rospy.logerr("Failed to call ReadAction") + return False + # Execute the HOME action if we could read it + else: + # What we just read is the input of the ExecuteAction service + req = ExecuteActionRequest() + req.input = res.output + rospy.loginfo("Sending the robot home...") + try: + self.execute_action(req) + except rospy.ServiceException: + rospy.logerr("Failed to call ExecuteAction") + return False + else: + return self.wait_for_action_end_or_abort() def main(): try: @@ -79,21 +84,19 @@ def main(): pass # Unpause the physics - # This will let MoveIt finish its initialization - rospy.loginfo("Unpausing") + rospy.loginfo("Unpausing Gazebo...") rospy.wait_for_service('/gazebo/unpause_physics') unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) resp = unpause_gazebo() - rospy.loginfo("Unpaused") + rospy.loginfo("Unpaused Gazebo.") example = ExampleInitializeGazeboRobot() - rospy.loginfo("Created example") success = example.is_init_success - rospy.loginfo("success = {}".format(success)) if success: success &= example.home_the_robot() - except: + except Exception as e: + print (e) success = False # For testing purposes @@ -104,4 +107,5 @@ def main(): rospy.loginfo("The Gazebo initialization executed without fail.") if __name__ == '__main__': + rospy.init_node('init_robot') main() diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant index af9086aa..8619322a 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_lite_gen3_lite_2f.xacro - xacro_args: "--inorder " + xacro_args: "" SRDF: relative_path: config/gen3_lite_gen3_lite_2f.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml index 8eda2826..60730a04 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml @@ -1,12 +1,12 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - right_finger_bottom_joint \ No newline at end of file + - $(arg prefix)right_finger_bottom_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf deleted file mode 100644 index 178ab9bc..00000000 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf +++ /dev/null @@ -1,107 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro new file mode 100644 index 00000000..bd5ef236 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml index 6bc8c8b7..66e05b12 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.35 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.17 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.14 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.35 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 3.5 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.55 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml index a71f1cae..2934c6dc 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_lite_joint_trajectory_controller" + - name: "$(arg prefix)gen3_lite_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "gen3_lite_2f_gripper_controller" + - name: "$(arg prefix)gen3_lite_2f_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - right_finger_bottom_joint \ No newline at end of file + - $(arg prefix)right_finger_bottom_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml index ff37a3ce..3b0aed09 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml index ec664d10..afb73408 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch index 5dfab46d..f26a35d8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -5,7 +5,12 @@ - + + + + + + @@ -46,6 +51,7 @@ + @@ -53,6 +59,7 @@ + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml index 7dc43ada..b8cd5b6a 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index f501959f..222a7a55 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -5,15 +5,22 @@ + + + - + - - + + + + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml index b941658f..6f172429 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml index 9a8e008c..7a09c04f 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml @@ -1,7 +1,7 @@ - + @@ -15,6 +15,8 @@ - + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml index 1e3add54..8e729e44 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/package.xml @@ -1,7 +1,7 @@ gen3_lite_gen3_lite_2f_move_it_config - 2.2.1 + 2.2.2 An automatically generated package with all the configuration and launch files for using the gen3_lite_gen3_lite_2f with the MoveIt! Motion Planning Framework diff --git a/kortex_move_it_config/gen3_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_move_it_config/.setup_assistant index f94315d9..94c2fcba 100644 --- a/kortex_move_it_config/gen3_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3.xacro - xacro_args: --inorder arm:=gen3 + xacro_args: arm:=gen3 SRDF: relative_path: config/gen3.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml index 6a93c689..7a51e2c1 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml index c693bcb7..84de5e8a 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml @@ -1,9 +1,9 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf deleted file mode 100644 index 586f19ba..00000000 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro new file mode 100644 index 00000000..1257563e --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml index 6783c6e2..f43b69a2 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml index 9048503d..221f9604 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml @@ -2,14 +2,14 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml index 68e9894d..056e61f7 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,37 +2,37 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml index 159da410..9bf3b0da 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml @@ -1,10 +1,10 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf deleted file mode 100644 index f7b254af..00000000 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro new file mode 100644 index 00000000..beb8c857 --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml index 642c2528..fdd996b5 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,37 +2,37 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml index 8a0e3f50..cfaa6205 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml @@ -2,15 +2,15 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml index 17829f90..1667cab7 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml @@ -146,5 +146,5 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml index d2f4eaf8..482742e6 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml index e0b4c42f..5b91ac16 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch index 20422937..56ffd49c 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch @@ -3,10 +3,14 @@ + + + + @@ -48,6 +52,7 @@ + @@ -56,6 +61,7 @@ + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml index 2ab04a16..bfe3d6c6 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index 0ef0c93b..21884a9f 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -8,19 +8,27 @@ + + + - + - + + + + - - + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml index 02c62e47..607842b2 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml index 7f071c1a..ebb523b9 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_move_it_config/package.xml b/kortex_move_it_config/gen3_move_it_config/package.xml index a409428c..f8040e4a 100644 --- a/kortex_move_it_config/gen3_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_move_it_config/package.xml @@ -1,7 +1,7 @@ gen3_move_it_config - 2.2.1 + 2.2.2 An automatically generated package with all the configuration and launch files for using the gen3 with the MoveIt! Motion Planning Framework diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant index e8017e1b..05b9cb92 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_robotiq_2f_85.xacro - xacro_args: --inorder arm:=gen3 gripper:=robotiq_2f_85 + xacro_args: arm:=gen3 gripper:=robotiq_2f_85 SRDF: relative_path: config/gen3_robotiq_2f_85.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml index 426d9e04..83fdb6bc 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml index 5b130b75..b9ec3932 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml @@ -1,11 +1,11 @@ - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf deleted file mode 100644 index e2cd635b..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf +++ /dev/null @@ -1,197 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro new file mode 100644 index 00000000..8521c61e --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml index eb4a100f..18bcded9 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml index b741c94d..f5d5e610 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "robotiq_2f_140_gripper_controller" + - name: "$(arg prefix)robotiq_2f_140_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml index f943baec..b172331f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml index ca87393a..57fb469b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml @@ -1,13 +1,13 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf deleted file mode 100644 index fd4a5cfd..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro new file mode 100644 index 00000000..cd5bb0e8 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml index 583d6682..7c291587 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml index 53436240..f1b8067b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml @@ -2,22 +2,22 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 - - name: "robotiq_2f_140_gripper_controller" + - name: "$(arg prefix)robotiq_2f_140_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml index f3a2be33..6ff1ed9f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -2,11 +2,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml index 0d27267f..6007253d 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch index 2a927a10..765335b7 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch @@ -4,9 +4,13 @@ + + + + @@ -48,6 +52,7 @@ + @@ -56,6 +61,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml index 56d02bf4..99ec2a50 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index a9676df5..84b0b00e 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -8,19 +8,27 @@ + + + - + - + + + + - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml index c2c6d8e2..4e99c9c6 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml index 92cfbd85..3eb4a38c 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml index bdd19f5f..50280983 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml @@ -1,7 +1,7 @@ gen3_robotiq_2f_140_move_it_config - 2.2.1 + 2.2.2 An automatically generated package with all the configuration and launch files for using the gen3_robotiq_2f_140 with the MoveIt! Motion Planning Framework diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant index e8017e1b..05b9cb92 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_robotiq_2f_85.xacro - xacro_args: --inorder arm:=gen3 gripper:=robotiq_2f_85 + xacro_args: arm:=gen3 gripper:=robotiq_2f_85 SRDF: relative_path: config/gen3_robotiq_2f_85.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml index 426d9e04..83fdb6bc 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml index 5b130b75..b9ec3932 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml @@ -1,11 +1,11 @@ - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf deleted file mode 100644 index 8ec5af66..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf +++ /dev/null @@ -1,198 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro new file mode 100644 index 00000000..0ff251e0 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro @@ -0,0 +1,199 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml index eb4a100f..18bcded9 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml index 792f1fc1..f13b3e57 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "robotiq_2f_85_gripper_controller" + - name: "$(arg prefix)robotiq_2f_85_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml index f943baec..b172331f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml index ca87393a..57fb469b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml @@ -1,13 +1,13 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf deleted file mode 100644 index a15f8f84..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro new file mode 100644 index 00000000..0463d2de --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml index 583d6682..7c291587 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml index 5b62d88e..f401d726 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml @@ -2,22 +2,22 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 - - name: "robotiq_2f_85_gripper_controller" + - name: "$(arg prefix)robotiq_2f_85_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml index 65ad9d3f..d9a09845 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -2,11 +2,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml index 8ba0c8f6..1d654026 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch index efaa685e..806b0a22 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch @@ -4,9 +4,13 @@ + + + + @@ -48,6 +52,7 @@ + @@ -56,6 +61,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml index 6f683627..5ef1a126 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 583764e0..5483e95a 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -7,20 +7,28 @@ + + + - + - + + + + - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml index ab340f3d..438586b2 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml index d8509a8f..55e216ca 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml index c5996940..1e215044 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml @@ -1,7 +1,7 @@ gen3_robotiq_2f_85_move_it_config - 2.2.1 + 2.2.2 An automatically generated package with all the configuration and launch files for using the gen3_robotiq_2f_85 with the MoveIt! Motion Planning Framework diff --git a/kortex_move_it_config/kortex_move_it_config/package.xml b/kortex_move_it_config/kortex_move_it_config/package.xml index 843b77b5..a9cbce90 100644 --- a/kortex_move_it_config/kortex_move_it_config/package.xml +++ b/kortex_move_it_config/kortex_move_it_config/package.xml @@ -1,12 +1,12 @@ kortex_move_it_config - 2.2.1 + 2.2.2 - The metapackage for the automatically generated MoveIt! configuration packages + The metapackage for the automatically generated MoveIt configuration packages Alexandre Vannobel - + BSD