Rename ServoServer to ServerNode (#649)

MIGRATION.md

@@ -2,6 +2,9 @@
 
 API changes in MoveIt releases
 
+## ROS Rolling
+- ServoServer was renamed to ServoNode
+
 ## ROS Noetic
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.
 - Planned trajectories are *slow* by default.

moveit_ros/moveit_servo/CMakeLists.txt

@@ -16,11 +16,11 @@ set(SERVO_PARAM_LIB_NAME ${SERVO_LIB_NAME}_parameters)
 set(POSE_TRACKING pose_tracking)
 
 # Component Nodes (Shared libraries) ############################
-set(SERVO_COMPONENT_NODE servo_server)
+set(SERVO_COMPONENT_NODE servo_node)
 set(SERVO_CONTROLLER_INPUT servo_controller_input)
 
 # Executable Nodes ##############################################
-set(SERVO_SERVER_NODE_NAME servo_server_node)
+set(SERVO_NODE_MAIN_NAME servo_node_main)
 set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
 set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
 
@@ -96,10 +96,10 @@ target_link_libraries(${POSE_TRACKING} ${SERVO_LIB_NAME})
 #####################
 
 # Add and export library to run as a ROS node component, and receive commands via topics
-add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_server.cpp)
+add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_node.cpp)
 ament_target_dependencies(${SERVO_COMPONENT_NODE} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_link_libraries(${SERVO_COMPONENT_NODE} ${SERVO_LIB_NAME})
-rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "moveit_servo::ServoServer")
+rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "moveit_servo::ServoNode")
 
 # Add executable for using a controller
 add_library(${SERVO_CONTROLLER_INPUT} SHARED src/teleop_demo/joystick_servo_example.cpp)
@@ -111,9 +111,9 @@ rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "moveit_servo::JoyToS
 ######################
 
 # An executable node for the servo server
-add_executable(${SERVO_SERVER_NODE_NAME} src/servo_server_node.cpp)
-target_link_libraries(${SERVO_SERVER_NODE_NAME} ${SERVO_COMPONENT_NODE})
-ament_target_dependencies(${SERVO_SERVER_NODE_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+add_executable(${SERVO_NODE_MAIN_NAME} src/servo_node_main.cpp)
+target_link_libraries(${SERVO_NODE_MAIN_NAME} ${SERVO_COMPONENT_NODE})
+ament_target_dependencies(${SERVO_NODE_MAIN_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # An example of pose tracking
 add_executable(${POSE_TRACKING_DEMO_NAME} src/cpp_interface_demo/pose_tracking_demo.cpp)
@@ -150,7 +150,7 @@ install(
 # Install Binaries
 install(
   TARGETS
-    ${SERVO_SERVER_NODE_NAME}
+    ${SERVO_NODE_MAIN_NAME}
     ${CPP_DEMO_NAME}
     ${POSE_TRACKING_DEMO_NAME}
     ${FAKE_SERVO_CMDS_NAME}

moveit_ros/moveit_servo/include/moveit_servo/servo_node.h

@@ -0,0 +1,88 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik Inc.
+ *  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 PickNik Inc. 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.
+ *********************************************************************/
+
+/*      Title     : servo_node.h
+ *      Project   : moveit_servo
+ *      Created   : 07/13/2020
+ *      Author    : Adam Pettinger
+ */
+
+#pragma once
+
+#include <moveit_servo/servo.h>
+#include <std_srvs/srv/trigger.hpp>
+
+namespace moveit_servo
+{
+class ServoNode : public rclcpp::Node
+{
+public:
+  ServoNode(const rclcpp::NodeOptions& options);
+
+private:
+  bool init();
+
+  void reset();
+
+  rclcpp::TimerBase::SharedPtr initialization_timer_;
+
+  std::unique_ptr<moveit_servo::Servo> servo_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
+
+  /** \brief Start the servo loop. Must be called once to begin Servoing. */
+  void startCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+               std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_servo_service_;
+
+  /** \brief Stop the servo loop. This involves more overhead than pauseCB, e.g. it clears the planning scene monitor.
+   * We recommend using pauseCB/unpauseCB if you will resume the Servo loop soon.
+   */
+  void stopCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+              std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_servo_service_;
+
+  /** \brief Pause the servo loop but continue monitoring joint state so we can resume easily. */
+  void pauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+               std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_servo_service_;
+
+  /** \brief Resume the servo loop after pauseCB has been called. */
+  void unpauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                 std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr unpause_servo_service_;
+
+  bool is_initialized_;
+};
+}  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/servo_server.h

@@ -40,49 +40,12 @@
 
 #pragma once
 
-#include <moveit_servo/servo.h>
-#include <std_srvs/srv/trigger.hpp>
+#pragma message("Header `servo_server.h` is deprecated, please use `servo_node.h`")
+
+#include <moveit_servo/servo_node.h>
 
 namespace moveit_servo
 {
-class ServoServer : public rclcpp::Node
-{
-public:
-  ServoServer(const rclcpp::NodeOptions& options);
-
-private:
-  bool init();
-
-  void reset();
-
-  rclcpp::TimerBase::SharedPtr initialization_timer_;
-
-  std::unique_ptr<moveit_servo::Servo> servo_;
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
-
-  /** \brief Start the servo loop. Must be called once to begin Servoing. */
-  void startCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-               std::shared_ptr<std_srvs::srv::Trigger::Response> response);
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_servo_service_;
-
-  /** \brief Stop the servo loop. This involves more overhead than pauseCB, e.g. it clears the planning scene monitor.
-   * We recommend using pauseCB/unpauseCB if you will resume the Servo loop soon.
-   */
-  void stopCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-              std::shared_ptr<std_srvs::srv::Trigger::Response> response);
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_servo_service_;
-
-  /** \brief Pause the servo loop but continue monitoring joint state so we can resume easily. */
-  void pauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-               std::shared_ptr<std_srvs::srv::Trigger::Response> response);
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_servo_service_;
-
-  /** \brief Resume the servo loop after pauseCB has been called. */
-  void unpauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-                 std::shared_ptr<std_srvs::srv::Trigger::Response> response);
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr unpause_servo_service_;
+using ServoServer [[deprecated("use ServoNode from servo_node.h")]] = ServoNode;
 
-  bool is_initialized_;
-};
 }  // namespace moveit_servo

moveit_ros/moveit_servo/src/servo_server.cpp → moveit_ros/moveit_servo/src/servo_node.cpp

@@ -31,20 +31,20 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *******************************************************************************/
 
-/*      Title     : servo_server.cpp
+/*      Title     : servo_node.cpp
  *      Project   : moveit_servo
  *      Created   : 12/31/2018
  *      Author    : Andy Zelenak
  */
 
-#include <moveit_servo/servo_server.h>
+#include <moveit_servo/servo_node.h>
 #include <moveit_servo/servo_parameters.h>
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_server");
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node");
 
 namespace moveit_servo
 {
-ServoServer::ServoServer(const rclcpp::NodeOptions& options) : Node("servo_service", options), is_initialized_(false)
+ServoNode::ServoNode(const rclcpp::NodeOptions& options) : Node("servo_node", options), is_initialized_(false)
 {
   if (!options.use_intra_process_comms())
   {
@@ -57,16 +57,16 @@ ServoServer::ServoServer(const rclcpp::NodeOptions& options) : Node("servo_servi
   using std::placeholders::_1;
   using std::placeholders::_2;
   start_servo_service_ =
-      this->create_service<std_srvs::srv::Trigger>("~/start_servo", std::bind(&ServoServer::startCB, this, _1, _2));
+      this->create_service<std_srvs::srv::Trigger>("~/start_servo", std::bind(&ServoNode::startCB, this, _1, _2));
   stop_servo_service_ =
-      this->create_service<std_srvs::srv::Trigger>("~/stop_servo", std::bind(&ServoServer::stopCB, this, _1, _2));
+      this->create_service<std_srvs::srv::Trigger>("~/stop_servo", std::bind(&ServoNode::stopCB, this, _1, _2));
   pause_servo_service_ =
-      this->create_service<std_srvs::srv::Trigger>("~/pause_servo", std::bind(&ServoServer::pauseCB, this, _1, _2));
+      this->create_service<std_srvs::srv::Trigger>("~/pause_servo", std::bind(&ServoNode::pauseCB, this, _1, _2));
   unpause_servo_service_ =
-      this->create_service<std_srvs::srv::Trigger>("~/unpause_servo", std::bind(&ServoServer::unpauseCB, this, _1, _2));
+      this->create_service<std_srvs::srv::Trigger>("~/unpause_servo", std::bind(&ServoNode::unpauseCB, this, _1, _2));
 }
 
-bool ServoServer::init()
+bool ServoNode::init()
 {
   bool performed_initialization = true;
 
@@ -119,16 +119,16 @@ bool ServoServer::init()
   }
 }
 
-void ServoServer::reset()
+void ServoNode::reset()
 {
   servo_.reset();
   tf_buffer_.reset();
   planning_scene_monitor_.reset();
   is_initialized_ = false;
 }
 
-void ServoServer::startCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-                          std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+void ServoNode::startCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                        std::shared_ptr<std_srvs::srv::Trigger::Response> response)
 {
   // If we already initialized, reset servo before initializing again
   if (is_initialized_)
@@ -137,24 +137,24 @@ void ServoServer::startCB(const std::shared_ptr<std_srvs::srv::Trigger::Request>
   response->success = init();
 }
 
-void ServoServer::stopCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-                         std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+void ServoNode::stopCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                       std::shared_ptr<std_srvs::srv::Trigger::Response> response)
 {
   reset();
   response->success = true;
 }
 
-void ServoServer::pauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-                          std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+void ServoNode::pauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                        std::shared_ptr<std_srvs::srv::Trigger::Response> response)
 {
   if (servo_)
     servo_->setPaused(true);
 
   response->success = true;
 }
 
-void ServoServer::unpauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
-                            std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+void ServoNode::unpauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                          std::shared_ptr<std_srvs::srv::Trigger::Response> response)
 {
   if (servo_)
     servo_->setPaused(false);
@@ -166,4 +166,4 @@ void ServoServer::unpauseCB(const std::shared_ptr<std_srvs::srv::Trigger::Reques
 
 // Register the component with class_loader
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoServer)
+RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode)

moveit_ros/moveit_servo/src/servo_server_node.cpp → moveit_ros/moveit_servo/src/servo_node_main.cpp

@@ -31,13 +31,13 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *******************************************************************************/
 
-/*      Title     : servo_server_node.cpp
+/*      Title     : servo_node_main.cpp
  *      Project   : moveit_servo
  *      Created   : 08/18/2021
  *      Author    : Joe Schornak
  */
 
-#include <moveit_servo/servo_server.h>
+#include <moveit_servo/servo_node.h>
 
 int main(int argc, char* argv[])
 {
@@ -46,7 +46,7 @@ int main(int argc, char* argv[])
   rclcpp::NodeOptions options;
   options.automatically_declare_parameters_from_overrides(true);
 
-  auto node = std::make_shared<moveit_servo::ServoServer>(options);
+  auto node = std::make_shared<moveit_servo::ServoNode>(options);
 
   rclcpp::spin(node);
 

moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp

@@ -49,8 +49,8 @@
 
 // We'll just set up parameters here
 const std::string JOY_TOPIC = "/joy";
-const std::string TWIST_TOPIC = "/servo_server/delta_twist_cmds";
-const std::string JOINT_TOPIC = "/servo_server/delta_joint_cmds";
+const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
+const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
 const size_t ROS_QUEUE_SIZE = 10;
 const std::string EEF_FRAME_ID = "panda_hand";
 const std::string BASE_FRAME_ID = "panda_link0";
@@ -165,8 +165,8 @@ class JoyToServoPub : public rclcpp::Node
     joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
     collision_pub_ = this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", 10);
 
-    // Create a service client to start the ServoServer
-    servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_server/start_servo");
+    // Create a service client to start the ServoNode
+    servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
     servo_start_client_->wait_for_service(std::chrono::seconds(1));
     servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
 

moveit_ros/moveit_servo/test/config/servo_settings.yaml

@@ -47,11 +47,11 @@ hard_stop_singularity_threshold: 45.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
-cartesian_command_in_topic: servo_server/delta_twist_cmds  # Topic for incoming Cartesian twist commands
-joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands
+cartesian_command_in_topic: servo_node/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: servo_node/delta_joint_cmds # Topic for incoming joint angle commands
 joint_topic:  joint_states
-status_topic: servo_server/status # Publish status to this topic
-command_out_topic: servo_server/command # Publish outgoing commands here
+status_topic: servo_node/status # Publish status to this topic
+command_out_topic: servo_node/command # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?