Skip to content

docs: improve MoveIt Servo Doxygen documentation #3714

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
vedbo wants to merge 2 commits into
base: main
Choose a base branch
from
+67 −119
Open

docs: improve MoveIt Servo Doxygen documentation #3714

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
102e40c
docs: document collision_monitor.hppRefactor comments and formatting …
vedbo Mar 28, 2026
a394866
docs: document servo_node.hppRefactor ServoNode for rclpp integration…
vedbo Mar 28, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 42 additions & 32 deletions moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
/*******************************************************************************
/*********************************************************************************
* BSD 3-Clause License
*
* Copyright (c) 2019, Los Alamos National Security, LLC
Expand All @@ -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.
Expand All @@ -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
Copy link
Copy Markdown
Contributor

@nbbrooks nbbrooks May 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These look like incorrect, ai generated changes.

* 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 <moveit_servo/servo.hpp>
#include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
#include <moveit/planning_scene/planning_scene.hpp>
#include <moveit_servo/moveit_servo_lib_parameters.hpp>

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<double>& 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<bool> stop_requested_;
// Thread controls
std::atomic<bool> 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<double>& 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
112 changes: 25 additions & 87 deletions moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
/*******************************************************************************
/*********************************************************************************
* BSD 3-Clause License
*
* Copyright (c) 2019, Los Alamos National Security, LLC
Expand All @@ -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.
Expand All @@ -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 <control_msgs/msg/joint_jog.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <moveit_msgs/srv/servo_command_type.hpp>
#include <moveit_msgs/msg/servo_status.hpp>
#include <moveit_servo/teleop_demo/joystick_servo_example.hpp>
#include <moveit_servo/servo.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <rclpp_rclpp.hpp>

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<std_srvs::srv::SetBool::Request>& request,
const std::shared_ptr<std_srvs::srv::SetBool::Response>& 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<moveit_msgs::srv::ServoCommandType::Request>& request,
const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& 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<KinematicState> processJointJogCommand(const moveit::core::RobotStatePtr& robot_state);
std::optional<KinematicState> processTwistCommand(const moveit::core::RobotStatePtr& robot_state);
std::optional<KinematicState> processPoseCommand(const moveit::core::RobotStatePtr& robot_state);

// Variables

const rclcpp::Node::SharedPtr node_;
private:
std::unique_ptr<Servo> 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<control_msgs::msg::JointJog>::SharedPtr joint_jog_subscriber_;
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_subscriber_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_subscriber_;

rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr multi_array_publisher_;
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
rclcpp::Publisher<moveit_msgs::msg::ServoStatus>::SharedPtr status_publisher_;

rclcpp::Service<moveit_msgs::srv::ServoCommandType>::SharedPtr switch_command_type_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr pause_servo_;

// Used for communication with thread
std::atomic<bool> stop_servo_;
std::atomic<bool> servo_paused_;
std::atomic<bool> 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<KinematicState> joint_cmd_rolling_window_;
rclpp_::Node::SharedPtr node_;
};

} // namespace moveit_servo