[Servo] Refactoring servo

moveit_ros/moveit_servo/CMakeLists.txt

@@ -7,7 +7,6 @@ moveit_package()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_msgs
-  control_toolbox
   geometry_msgs
   moveit_core
   moveit_msgs
@@ -42,60 +41,59 @@ include_directories(
 # This library provides a way of loading parameters for servo
 generate_parameter_library(
   moveit_servo_lib_parameters
-  src/servo_parameters.yaml
+  config/servo_parameters.yaml
 )
 
 # This library provides a C++ interface for sending realtime twist or joint commands to a robot
-add_library(moveit_servo_lib SHARED
-  src/collision_check.cpp
+add_library(moveit_servo_lib_cpp SHARED
+  src/collision_monitor.cpp
   src/servo.cpp
-  src/servo_calcs.cpp
-  src/utilities.cpp
-)
-set_target_properties(moveit_servo_lib PROPERTIES VERSION "${moveit_servo_VERSION}")
-ament_target_dependencies(moveit_servo_lib ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_link_libraries(moveit_servo_lib moveit_servo_lib_parameters)
+  src/utils/common.cpp
+  src/utils/command.cpp
 
-add_library(pose_tracking SHARED src/pose_tracking.cpp)
-ament_target_dependencies(pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_link_libraries(pose_tracking moveit_servo_lib)
+)
+set_target_properties(moveit_servo_lib_cpp PROPERTIES VERSION "${moveit_servo_VERSION}")
+target_link_libraries(moveit_servo_lib_cpp moveit_servo_lib_parameters)
+ament_target_dependencies(moveit_servo_lib_cpp ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-#####################
-## Component Nodes ##
-#####################
+add_library(moveit_servo_lib_ros SHARED src/servo_node.cpp)
+set_target_properties(moveit_servo_lib_ros PROPERTIES VERSION "${moveit_servo_VERSION}")
+target_link_libraries(moveit_servo_lib_ros moveit_servo_lib_cpp)
+ament_target_dependencies(moveit_servo_lib_ros ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-# Add and export library to run as a ROS node component, and receive commands via topics
-add_library(servo_node SHARED src/servo_node.cpp)
-ament_target_dependencies(servo_node ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_link_libraries(servo_node moveit_servo_lib)
-rclcpp_components_register_nodes(servo_node "moveit_servo::ServoNode")
+######################
+## Components ##
+######################
 
-# Add executable for using a controller
-add_library(servo_controller_input SHARED src/teleop_demo/joystick_servo_example.cpp)
-ament_target_dependencies(servo_controller_input PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
-rclcpp_components_register_nodes(servo_controller_input "moveit_servo::JoyToServoPub")
+rclcpp_components_register_node(
+  moveit_servo_lib_ros
+  PLUGIN "moveit_servo::ServoNode"
+  EXECUTABLE servo_node
+)
 
 ######################
 ## Executable Nodes ##
 ######################
 
-# An executable node for the servo server
-add_executable(servo_node_main src/servo_node_main.cpp)
-target_link_libraries(servo_node_main servo_node)
-ament_target_dependencies(servo_node_main ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# Executable node for the joint jog demo
+add_executable(demo_joint_jog demos/cpp_interface/demo_joint_jog.cpp )
+target_link_libraries(demo_joint_jog moveit_servo_lib_cpp)
+ament_target_dependencies(demo_joint_jog ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-# An example of pose tracking
-add_executable(servo_pose_tracking_demo src/cpp_interface_demo/pose_tracking_demo.cpp)
-target_link_libraries(servo_pose_tracking_demo pose_tracking)
-ament_target_dependencies(servo_pose_tracking_demo ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# Executable node for the twist demo
+add_executable(demo_twist demos/cpp_interface/demo_twist.cpp )
+target_link_libraries(demo_twist moveit_servo_lib_cpp)
+ament_target_dependencies(demo_twist ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-# Add executable to publish fake servo commands for testing/demo purposes
-add_executable(fake_command_publisher test/publish_fake_jog_commands.cpp)
-ament_target_dependencies(fake_command_publisher
-  rclcpp
-  geometry_msgs
-  std_srvs
-)
+# Executable node for the pose demo
+add_executable(demo_pose demos/cpp_interface/demo_pose.cpp )
+target_link_libraries(demo_pose moveit_servo_lib_cpp)
+ament_target_dependencies(demo_pose ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Keyboard control example for servo
+add_executable(servo_keyboard_input demos/servo_keyboard_input.cpp)
+target_include_directories(servo_keyboard_input PUBLIC include)
+ament_target_dependencies(servo_keyboard_input  ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 #############
 ## Install ##
@@ -104,11 +102,9 @@ ament_target_dependencies(fake_command_publisher
 # Install Libraries
 install(
   TARGETS
-    moveit_servo_lib
+    moveit_servo_lib_cpp
+    moveit_servo_lib_ros
     moveit_servo_lib_parameters
-    pose_tracking
-    servo_node
-    servo_controller_input
   EXPORT moveit_servoTargets
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
@@ -119,9 +115,11 @@ install(
 # Install Binaries
 install(
   TARGETS
-    servo_node_main
-    servo_pose_tracking_demo
-    fake_command_publisher
+  demo_joint_jog
+  demo_twist
+  demo_pose
+  servo_node
+  servo_keyboard_input
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/moveit_servo
@@ -135,56 +133,25 @@ install(DIRECTORY config DESTINATION share/moveit_servo)
 ament_export_targets(moveit_servoTargets HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-#############
-## TESTING ##
-#############
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  find_package(ros_testing REQUIRED)
-  find_package(Boost REQUIRED COMPONENTS filesystem)
-
-  # These don't pass yet, disable them for now
-  set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cpplint_FOUND TRUE)
-  set(ament_cmake_flake8_FOUND TRUE)
-  set(ament_cmake_uncrustify_FOUND TRUE)
-
-  # Run all lint tests in package.xml except those listed above
-  ament_lint_auto_find_test_dependencies()
-
-  # Servo integration launch test
-  ament_add_gtest_executable(test_servo_integration
-      test/test_servo_interface.cpp
-      test/servo_launch_test_common.hpp
-  )
-  target_link_libraries(test_servo_integration moveit_servo_lib_parameters)
-  ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # Servo collision checking integration test
-  ament_add_gtest_executable(test_servo_collision
-        test/test_servo_collision.cpp
-        test/servo_launch_test_common.hpp
-  )
-  target_link_libraries(test_servo_collision moveit_servo_lib_parameters)
-  ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # pose_tracking
-  ament_add_gtest_executable(test_servo_pose_tracking
-        test/pose_tracking_test.cpp
-  )
-  ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  target_link_libraries(test_servo_pose_tracking pose_tracking)
-  add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # Unit tests
-  ament_add_gtest(servo_calcs_unit_tests
-    test/servo_calcs_unit_tests.cpp
-  )
-  target_link_libraries(servo_calcs_unit_tests moveit_servo_lib)
+
+    find_package(ament_cmake_gtest REQUIRED)
+    find_package(ros_testing REQUIRED)
+
+    ament_add_gtest_executable(moveit_servo_utils_test tests/test_utils.cpp)
+    target_link_libraries(moveit_servo_utils_test moveit_servo_lib_cpp)
+    ament_target_dependencies(moveit_servo_utils_test ${THIS_PACKAGE_INCLUDE_DEPENDS})
+    add_ros_test(tests/launch/servo_utils.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+    ament_add_gtest_executable(moveit_servo_cpp_integration_test tests/test_integration.cpp tests/servo_cpp_fixture.hpp)
+    target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp)
+    ament_target_dependencies(moveit_servo_cpp_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS})
+    add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+    ament_add_gtest_executable(moveit_servo_ros_integration_test tests/test_ros_integration.cpp tests/servo_ros_fixture.hpp)
+    ament_target_dependencies(moveit_servo_ros_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS})
+    add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
 endif()
 

moveit_ros/moveit_servo/config/demo_rviz_config.rviz

@@ -46,7 +46,7 @@ Visualization Manager:
       Enabled: true
       Move Group Namespace: ""
       Name: PlanningScene
-      Planning Scene Topic: /servo_node/publish_planning_scene
+      Planning Scene Topic: /moveit_servo_demo/publish_planning_scene
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 0.8999999761581421

moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz → moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz

@@ -46,7 +46,7 @@ Visualization Manager:
       Enabled: true
       Move Group Namespace: ""
       Name: PlanningScene
-      Planning Scene Topic: /monitored_planning_scene
+      Planning Scene Topic: /servo_node/publish_planning_scene
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 0.8999999761581421

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -2,22 +2,20 @@
 # Modify all parameters related to servoing here
 ###############################################
 
-## Properties of incoming commands
-command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
+## Properties of outgoing commands
+publish_period: 0.034  # 1/Nominal publish rate [seconds]
+
+command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
   linear:  0.4  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
   rotational:  0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
   joint: 0.5
 
-# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
-# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
-
-## Properties of outgoing commands
-publish_period: 0.034  # 1/Nominal publish rate [seconds]
-low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
-
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
 command_out_type: trajectory_msgs/JointTrajectory
@@ -28,6 +26,7 @@ publish_joint_velocities: true
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
+use_smoothing: true
 smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
 
 # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
@@ -41,11 +40,7 @@ move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
 planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
-ee_frame_name: panda_link8  # The name of the end effector link, used to return the EE pose
-robot_link_command_frame:  panda_link0  # commands must be given in the frame of a robot link. Usually either the base or end effector
-
-## Stopping behaviour
-incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
+ee_frame: panda_link8  # The name of the end effector link, used to return the EE pose
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)