Update tests + clang-tidy

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -8,6 +8,7 @@
 ## Properties of outgoing commands
 publish_period: 0.02  # 1/Nominal publish rate [seconds]
 
+incoming_command_timeout: 0.5 # seconds
 command_in_type: "unitless" # "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"

moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp

@@ -83,34 +83,34 @@ const std::string PLANNING_FRAME_ID = "panda_link0";
 class KeyboardReader
 {
 public:
-  KeyboardReader() : kfd(0)
+  KeyboardReader() : file_descriptor_(0)
   {
     // get the console in raw mode
-    tcgetattr(kfd, &cooked);
+    tcgetattr(file_descriptor_, &cooked_);
     struct termios raw;
-    memcpy(&raw, &cooked, sizeof(struct termios));
+    memcpy(&raw, &cooked_, sizeof(struct termios));
     raw.c_lflag &= ~(ICANON | ECHO);
     // Setting a new line, then end of file
     raw.c_cc[VEOL] = 1;
     raw.c_cc[VEOF] = 2;
-    tcsetattr(kfd, TCSANOW, &raw);
+    tcsetattr(file_descriptor_, TCSANOW, &raw);
   }
   void readOne(char* c)
   {
-    int rc = read(kfd, c, 1);
+    int rc = read(file_descriptor_, c, 1);
     if (rc < 0)
     {
       throw std::runtime_error("read failed");
     }
   }
   void shutdown()
   {
-    tcsetattr(kfd, TCSANOW, &cooked);
+    tcsetattr(file_descriptor_, TCSANOW, &cooked_);
   }
 
 private:
-  int kfd;
-  struct termios cooked;
+  int file_descriptor_;
+  struct termios cooked_;
 };
 
 // Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller
@@ -182,7 +182,7 @@ int KeyboardServo::keyLoop()
   bool publish_twist = false;
   bool publish_joint = false;
 
-  std::thread{ std::bind(&KeyboardServo::spin, this) }.detach();
+  std::thread{ [this]() { return spin(); } }.detach();
 
   puts("Reading from keyboard");
   puts("---------------------------");
@@ -298,9 +298,13 @@ int KeyboardServo::keyLoop()
         {
           auto result = switch_input_->async_send_request(request_);
           if (result.get()->success)
+          {
             RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: JointJog");
+          }
           else
+          {
             RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: JointJog");
+          }
         }
         break;
       case KEYCODE_T:
@@ -311,9 +315,13 @@ int KeyboardServo::keyLoop()
         {
           auto result = switch_input_->async_send_request(request_);
           if (result.get()->success)
+          {
             RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: Twist");
+          }
           else
+          {
             RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: Twist");
+          }
         }
         break;
       case KEYCODE_Q:

moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp

@@ -81,20 +81,20 @@ class ServoNode
    * \brief The service to pause servoing, this does not exit the loop or stop the servo loop thread.
    * The loop will be alive even after pausing, but no commands will be processed.
    */
-  void pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
-                  const std::shared_ptr<std_srvs::srv::SetBool::Response> response);
+  void pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                  const std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
 
   /**
    * \brief The service to set the command type for Servo.
    * Supported command types can be found in utils/datatypes.hpp
    * This service must be used to set the command type before sending any servoing commands.
    */
-  void switchCommandType(const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request> request,
-                         const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response> response);
+  void switchCommandType(const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
+                         const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response);
 
-  void jointJogCallback(const control_msgs::msg::JointJog::SharedPtr msg);
-  void twistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
-  void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+  void jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg);
+  void twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg);
+  void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg);
 
   std::optional<KinematicState> processJointJogCommand();
   std::optional<KinematicState> processTwistCommand();

moveit_ros/moveit_servo/src/servo.cpp

@@ -55,7 +55,7 @@ namespace moveit_servo
 Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::ParamListener> servo_param_listener,
              const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
   : node_(node)
-  , servo_param_listener_{ servo_param_listener }
+  , servo_param_listener_{ std::move(servo_param_listener) }
   , planning_scene_monitor_{ planning_scene_monitor }
   , transform_buffer_(node_->get_clock())
   , transform_listener_(transform_buffer_)

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -80,9 +80,13 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options)
   if (realtime_tools::has_realtime_kernel())
   {
     if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority))
+    {
       RCLCPP_INFO_STREAM(LOGGER, "Realtime kernel available, higher thread priority has been set.");
+    }
     else
+    {
       RCLCPP_WARN_STREAM(LOGGER, "Could not enable FIFO RT scheduling policy.");
+    }
   }
   else
   {
@@ -100,42 +104,53 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options)
 
   // Create subscriber for jointjog
   joint_jog_subscriber_ = node_->create_subscription<control_msgs::msg::JointJog>(
-      servo_params_.joint_command_in_topic, 10, std::bind(&ServoNode::jointJogCallback, this, std::placeholders::_1));
+      servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(),
+      [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointJogCallback(msg); });
 
   // Create subscriber for twist
   twist_subscriber_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
-      servo_params_.cartesian_command_in_topic, 10, std::bind(&ServoNode::twistCallback, this, std::placeholders::_1));
+      servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(),
+      [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { return twistCallback(msg); });
 
   // Create subscriber for pose
   pose_subscriber_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
-      servo_params_.pose_command_in_topic, 10, std::bind(&ServoNode::poseCallback, this, std::placeholders::_1));
+      servo_params_.pose_command_in_topic, rclcpp::SystemDefaultsQoS(),
+      [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { return poseCallback(msg); });
 
   if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory")
+  {
     trajectory_publisher_ = node_->create_publisher<trajectory_msgs::msg::JointTrajectory>(
         servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS());
+  }
   else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray")
+  {
     multi_array_publisher_ = node_->create_publisher<std_msgs::msg::Float64MultiArray>(servo_params_.command_out_topic,
                                                                                        rclcpp::SystemDefaultsQoS());
-
+  }
   // Create status publisher
   status_publisher_ =
       node_->create_publisher<moveit_msgs::msg::ServoStatus>(servo_params_.status_topic, rclcpp::SystemDefaultsQoS());
 
   // Create service to enable switching command type
   switch_command_type_ = node_->create_service<moveit_msgs::srv::ServoCommandType>(
-      "~/switch_command_type",
-      std::bind(&ServoNode::switchCommandType, this, std::placeholders::_1, std::placeholders::_2));
+      "~/switch_command_type", [this](const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
+                                      const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response) {
+        return switchCommandType(request, response);
+      });
 
   // Create service to pause/unpause servoing
   pause_servo_ = node_->create_service<std_srvs::srv::SetBool>(
-      "~/pause_servo", std::bind(&ServoNode::pauseServo, this, std::placeholders::_1, std::placeholders::_2));
+      "~/pause_servo", [this](const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                              const std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
+        return pauseServo(request, response);
+      });
 
   // Start the servoing loop
   servo_loop_thread_ = std::thread(&ServoNode::servoLoop, this);
 }
 
-void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
-                           const std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                           const std::shared_ptr<std_srvs::srv::SetBool::Response>& response)
 {
   servo_paused_ = request->data;
   response->success = (servo_paused_ == request->data);
@@ -151,8 +166,8 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
   }
 }
 
-void ServoNode::switchCommandType(const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request> request,
-                                  const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response> response)
+void ServoNode::switchCommandType(const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
+                                  const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response)
 {
   const bool is_valid = (request->command_type >= static_cast<int8_t>(CommandType::MIN)) &&
                         (request->command_type <= static_cast<int8_t>(CommandType::MAX));
@@ -167,19 +182,19 @@ void ServoNode::switchCommandType(const std::shared_ptr<moveit_msgs::srv::ServoC
   response->success = (request->command_type == static_cast<int8_t>(servo_->getCommandType()));
 }
 
-void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::SharedPtr msg)
+void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg)
 {
   latest_joint_jog_ = *msg;
   new_joint_jog_msg_ = true;
 }
 
-void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
+void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg)
 {
   latest_twist_ = *msg;
   new_twist_msg_ = true;
 }
 
-void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg)
 {
   latest_pose_ = *msg;
   new_pose_msg_ = true;
@@ -295,17 +310,17 @@ void ServoNode::servoLoop()
       continue;
 
     next_joint_state = std::nullopt;
-    const CommandType expectedType = servo_->getCommandType();
+    const CommandType expected_type = servo_->getCommandType();
 
-    if (expectedType == CommandType::JOINT_JOG && new_joint_jog_msg_)
+    if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_)
     {
       next_joint_state = processJointJogCommand();
     }
-    else if (expectedType == CommandType::TWIST && new_twist_msg_)
+    else if (expected_type == CommandType::TWIST && new_twist_msg_)
     {
       next_joint_state = processTwistCommand();
     }
-    else if (expectedType == CommandType::POSE && new_pose_msg_)
+    else if (expected_type == CommandType::POSE && new_pose_msg_)
     {
       next_joint_state = processPoseCommand();
     }

moveit_ros/moveit_servo/src/utils/command.cpp

@@ -147,10 +147,14 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, moveit::core::
   {
     status = StatusCode::INVALID;
     if (!valid_command)
+    {
       RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command.");
+    }
     if (!is_planning_frame)
+    {
       RCLCPP_WARN_STREAM(LOGGER,
                          "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame);
+    }
   }
   return std::make_pair(status, joint_position_delta);
 }
@@ -191,10 +195,14 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, moveit::core::Ro
   {
     status = StatusCode::INVALID;
     if (!valid_command)
+    {
       RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command.");
+    }
     if (!is_planning_frame)
+    {
       RCLCPP_WARN_STREAM(LOGGER,
                          "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame);
+    }
   }
   return std::make_pair(status, joint_position_delta);
 }

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -333,6 +333,7 @@ planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const
   // Set up planning_scene_monitor
   planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(node, robot_description_name,
                                                                                           "planning_scene_monitor");
+
   planning_scene_monitor->startStateMonitor(servo_params.joint_topic);
   planning_scene_monitor->startSceneMonitor(servo_params.monitored_planning_scene_topic);
   planning_scene_monitor->setPlanningScenePublishingFrequency(25);

moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py

@@ -111,9 +111,7 @@ def generate_test_description():
             launch_testing.actions.ReadyToTest(),
         ]
     ), {
-        "test_container": test_container,
         "servo_gtest": servo_gtest,
-        "ros2_control_node": ros2_control_node,
     }