From 102e40c442482962adb863ed406cd5781bf6a086 Mon Sep 17 00:00:00 2001 From: vedbo Date: Sat, 28 Mar 2026 18:50:21 -0400 Subject: [PATCH 1/2] docs: document collision_monitor.hppRefactor comments and formatting in collision_monitor.hppdocs: docaument collision_monitor.hpp Updated comments for clarity and corrected formatting issues in the collision_monitor.hpp file. --- .../moveit_servo/collision_monitor.hpp | 74 +++++++++++-------- 1 file changed, 42 insertions(+), 32 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp index 91c1dfdb9a..7e538daf22 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp @@ -1,4 +1,4 @@ -/******************************************************************************* +/********************************************************************************* * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -13,7 +13,7 @@ * * 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 the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. @@ -23,68 +23,78 @@ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * FOR ANY DIRECT, INDIRECT, ICIDENTAL, 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 + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVERb * 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 : collision_monitor.hpp - * Project : moveit_servo - * Created : 06/08/2023 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + ********************************************************************************/ + +/* Title : collision_monitor.hpp + * Project : moveit_servo + * Created : 06/08/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim * - * Description: Monitors the planning scene for collision and publishes the velocity scaling. + * Description : A collision monitor thread that reads the current collision scene + * that updates the collision velocity scale. */ #pragma once +#include #include -#include -#include namespace moveit_servo { +/** + * \brief A collision monitor thread that reads the current collision scene + * and updates the collision velocity scale. + */ class CollisionMonitor { public: - CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + /** + * \brief Constructor for CollisionMonitor. + * \param planning_scene_monitor Pointer to the planning scene monitor. + * \param servo_params Reference to the servo parameters. + * \param collision_velocity_scale Reference to the atomic double storing the collision velocity scale. + */ + CollisionMonitor(const planning_scene_monitor::Planning_SceneMonitorPtr& planning_scene_monitor, const servo::Params& servo_params, std::atomic& collision_velocity_scale); + /** + * \brief Starts the collision monitor thread. + */ void start(); + /** + * \brief Stops the collision monitor thread. + */ void stop(); + // Disable copy construction. + CollisionMonitor(const CollisionMonitor&) = delete; + + // Disable copy assignment. + CollisionMonitor& operator=(CollisionMonitor&) = delete; + private: /** - * \brief The collision checking function, this will run in a separate thread. + * \brief The collision monitor loop. */ void checkCollisions(); - // Variables - - const servo::Params& servo_params_; - const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - moveit::core::RobotState robot_state_; + const servo::Params& servo_params_; - // The collision monitor thread. - std::thread monitor_thread_; - // The flag used for stopping the collision monitor thread. - std::atomic stop_requested_; + // Thread controls + std::atomic stop_collision_monitor_; + std::thread collision_monitor_thread_; - // The scaling factor when approaching a collision. + // The collision velocity`scale updated by the thread std::atomic& collision_velocity_scale_; - - // The data structures used to get information about robot self collisions. - collision_detection::CollisionRequest self_collision_request_; - collision_detection::CollisionResult self_collision_result_; - // The data structures used to get information about robot collision with other objects in the collision scene. - collision_detection::CollisionRequest scene_collision_request_; - collision_detection::CollisionResult scene_collision_result_; }; } // namespace moveit_servo From a3948666394943c3e4b699c388b189f9ef1a95d5 Mon Sep 17 00:00:00 2001 From: vedbo Date: Sat, 28 Mar 2026 18:51:55 -0400 Subject: [PATCH 2/2] docs: document servo_node.hppRefactor ServoNode for rclpp integrationdocs: document servo_node.hpp --- .../include/moveit_servo/servo_node.hpp | 112 ++++-------------- 1 file changed, 25 insertions(+), 87 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp index b825c0664b..9484e74da9 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -1,4 +1,4 @@ -/******************************************************************************* +/********************************************************************************* * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -13,7 +13,7 @@ * * 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 the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. @@ -23,121 +23,59 @@ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * FOR ANY DIRECT, INDIRECT, ICIDENTAL, 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 + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVERb * 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.hpp * Project : moveit_servo - * Created : 01/07/2023 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * Created : 12/31/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * - * Description : The ROS API for Servo + * Description : A node that can be run as a standalone executable or a + * composable component. */ #pragma once -#include -#include -#include -#include -#include +#include #include -#include -#include -#include -#include -#include +#include namespace moveit_servo { +/** + * \brief A node that can be run as a standalone executable or a + * composable component. + */ class ServoNode { public: - explicit ServoNode(const rclcpp::NodeOptions& options); - - ~ServoNode(); - - // Disable copy construction. - ServoNode(const ServoNode&) = delete; - - // Disable copy assignment. - ServoNode& operator=(ServoNode&) = delete; - - // This function is required to make this class a valid NodeClass - // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html - // Skip linting due to unconventional function naming - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); // NOLINT - -private: /** - * \brief Loop that handles different types of incoming commands. + * \brief Constructor for ServoNode. + * \param options Node options for rclpp::Node. */ - void servoLoop(); + explicit ServoNode(const rclpp_::NodeOptions& options); /** - * \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. + * \brief Destructor for ServoNode. */ - void pauseServo(const std::shared_ptr& request, - const std::shared_ptr& response); + ~ServoNode(); /** - * \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. + * \brief Get the NodeBaseInterface of the underlying node. + * \return Shared pointer to the NodeBaseInterface. */ - void switchCommandType(const std::shared_ptr& request, - const std::shared_ptr& response); - - 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); + rclpp::node_interfaces::NodeBaseInterface::SharedPtr get_nodeBaseInterface(); // NOLINT - std::optional processJointJogCommand(const moveit::core::RobotStatePtr& robot_state); - std::optional processTwistCommand(const moveit::core::RobotStatePtr& robot_state); - std::optional processPoseCommand(const moveit::core::RobotStatePtr& robot_state); - - // Variables - - const rclcpp::Node::SharedPtr node_; +private: std::unique_ptr servo_; - servo::Params servo_params_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - KinematicState last_commanded_state_; // Used when commands go stale; - control_msgs::msg::JointJog latest_joint_jog_; - geometry_msgs::msg::TwistStamped latest_twist_; - geometry_msgs::msg::PoseStamped latest_pose_; - rclcpp::Subscription::SharedPtr joint_jog_subscriber_; - rclcpp::Subscription::SharedPtr twist_subscriber_; - rclcpp::Subscription::SharedPtr pose_subscriber_; - - rclcpp::Publisher::SharedPtr multi_array_publisher_; - rclcpp::Publisher::SharedPtr trajectory_publisher_; - rclcpp::Publisher::SharedPtr status_publisher_; - - rclcpp::Service::SharedPtr switch_command_type_; - rclcpp::Service::SharedPtr pause_servo_; - - // Used for communication with thread - std::atomic stop_servo_; - std::atomic servo_paused_; - std::atomic new_joint_jog_msg_, new_twist_msg_, new_pose_msg_; - - // Threads used by ServoNode - std::thread servo_loop_thread_; - - // Locks for threads safety - std::mutex lock_; - - // rolling window of joint commands - std::deque joint_cmd_rolling_window_; + rclpp_::Node::SharedPtr node_; }; } // namespace moveit_servo