Rewrite collision monitor

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -240,7 +240,7 @@ servo:
     description: "[Hz] Collision-checking can easily bog down a CPU if done too often. \
                   Collision checking begins slowing down when nearer than a specified distance.",
     validation: {
-      gt_eq<>: 0.0
+      gt_eq<>: 10.0
     }
   }
 

moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp

@@ -33,91 +33,49 @@
 /*
  * Title      : collision_monitor.hpp
  * Project    : moveit_servo
- * Created    : 1/11/2019
- * Author     : Brian O'Neil, Andy Zelenak, Blake Anderson
+ * Created    : 08/06/2023
+ * Author     : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim
  *
  * Description: Monitors the planning scene for collision and publishes the velocity scaling.
  */
 
 #pragma once
 
-#include <mutex>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <moveit/robot_model_loader/robot_model_loader.h>
-#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
-#include <sensor_msgs/msg/joint_state.hpp>
-#include <std_msgs/msg/float64.hpp>
-// Auto-generated
 #include <moveit_servo_lib_parameters.hpp>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
 namespace moveit_servo
 {
-class CollisionCheck
+
+class CollisionMonitor
 {
 public:
-  /** \brief Constructor
-   *  \param parameters: common settings of moveit_servo
-   *  \param planning_scene_monitor: PSM should have scene monitor and state monitor
-   *                                 already started when passed into this class
-   */
-  CollisionCheck(const rclcpp::Node::SharedPtr& node,
-                 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                 const std::shared_ptr<const servo::ParamListener>& servo_param_listener);
-
-  ~CollisionCheck()
-  {
-    stop();
-  }
+  CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                   const servo::Params& servo_params, std::atomic<double>& collision_velocity_scale);
 
-  /** \brief Start the Timer that regulates collision check rate */
   void start();
 
-  /** \brief Stop the Timer that regulates collision check rate */
   void stop();
 
 private:
-  /** \brief Run one iteration of collision checking */
-  void run();
+  void checkCollisions();
 
-  /** \brief Get a read-only copy of the planning scene */
-  planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const;
+  // Variables
+  std::atomic<bool> stop_requested_;
+  std::thread monitor_thread_;
 
-  // Pointer to the ROS node
-  const std::shared_ptr<rclcpp::Node> node_;
+  std::atomic<double>& collision_velocity_scale_;
+  const servo::Params& servo_params_;
 
-  // Servo parameters
-  const std::shared_ptr<const servo::ParamListener> servo_param_listener_;
-  servo::Params servo_params_;
+  collision_detection::CollisionRequest self_collision_request_;
+  collision_detection::CollisionResult self_collision_result_;
 
-  // Pointer to the collision environment
-  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+  collision_detection::CollisionRequest scene_collision_request_;
+  collision_detection::CollisionResult scene_collision_result_;
 
-  // Robot state and collision matrix from planning scene
-  std::shared_ptr<moveit::core::RobotState> robot_state_;
-
-  // Scale robot velocity according to collision proximity and user-defined thresholds.
-  // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold.
-  // Proximity decreasing --> decelerate
-  double velocity_scale_ = 1.0;
-  double self_collision_distance_ = 0.0;
-  double scene_collision_distance_ = 0.0;
-  bool collision_detected_ = false;
-
-  const double self_velocity_scale_coefficient_;
-  const double scene_velocity_scale_coefficient_;
-
-  // collision request
-  collision_detection::CollisionRequest collision_request_;
-  collision_detection::CollisionResult collision_result_;
-
-  // ROS
-  rclcpp::TimerBase::SharedPtr timer_;
-  double period_;  // The loop period, in seconds
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr collision_velocity_scale_pub_;
-
-  mutable std::mutex joint_state_mutex_;
-  sensor_msgs::msg::JointState latest_joint_state_;
+  moveit::core::RobotStatePtr robot_state_;
+  const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 };
+
 }  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/collision_monitor_old.hpp

@@ -0,0 +1,123 @@
+/*******************************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2021, 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 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.
+ *
+ * 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 HOLDER 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      : collision_monitor.hpp
+ * Project    : moveit_servo
+ * Created    : 1/11/2019
+ * Author     : Brian O'Neil, Andy Zelenak, Blake Anderson
+ *
+ * Description: Monitors the planning scene for collision and publishes the velocity scaling.
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <std_msgs/msg/float64.hpp>
+// Auto-generated
+#include <moveit_servo_lib_parameters.hpp>
+
+namespace moveit_servo
+{
+class CollisionCheck
+{
+public:
+  /** \brief Constructor
+   *  \param parameters: common settings of moveit_servo
+   *  \param planning_scene_monitor: PSM should have scene monitor and state monitor
+   *                                 already started when passed into this class
+   */
+  CollisionCheck(const rclcpp::Node::SharedPtr& node,
+                 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                 const std::shared_ptr<const servo::ParamListener>& servo_param_listener);
+
+  ~CollisionCheck()
+  {
+    stop();
+  }
+
+  /** \brief Start the Timer that regulates collision check rate */
+  void start();
+
+  /** \brief Stop the Timer that regulates collision check rate */
+  void stop();
+
+private:
+  /** \brief Run one iteration of collision checking */
+  void run();
+
+  /** \brief Get a read-only copy of the planning scene */
+  planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const;
+
+  // Pointer to the ROS node
+  const std::shared_ptr<rclcpp::Node> node_;
+
+  // Servo parameters
+  const std::shared_ptr<const servo::ParamListener> servo_param_listener_;
+  servo::Params servo_params_;
+
+  // Pointer to the collision environment
+  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+
+  // Robot state and collision matrix from planning scene
+  std::shared_ptr<moveit::core::RobotState> robot_state_;
+
+  // Scale robot velocity according to collision proximity and user-defined thresholds.
+  // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold.
+  // Proximity decreasing --> decelerate
+  double velocity_scale_ = 1.0;
+  double self_collision_distance_ = 0.0;
+  double scene_collision_distance_ = 0.0;
+  bool collision_detected_ = false;
+
+  const double self_velocity_scale_coefficient_;
+  const double scene_velocity_scale_coefficient_;
+
+  // collision request
+  collision_detection::CollisionRequest collision_request_;
+  collision_detection::CollisionResult collision_result_;
+
+  // ROS
+  rclcpp::TimerBase::SharedPtr timer_;
+  double period_;  // The loop period, in seconds
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr collision_velocity_scale_pub_;
+
+  mutable std::mutex joint_state_mutex_;
+  sensor_msgs::msg::JointState latest_joint_state_;
+};
+}  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -68,6 +68,8 @@ class Servo
 public:
   Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::ParamListener>& servo_param_listener);
 
+  ~Servo();
+
   /**
    * \brief Computes the required joint state to follow the given command.
    * @param command The command to follow, std::variant type, can handle JointJog, Twist and Pose.
@@ -128,6 +130,12 @@ class Servo
    */
   Twist toPlanningFrame(const Twist& command);
 
+  /**
+   * \brief Start/Stop collision checking thread.
+   * @param check_collision Stops collision checking thread if false, starts it if true.
+   */
+  void setCollisionChecking(const bool check_collision);
+
 private:
   /**
    * \brief Validate the servo parameters
@@ -145,11 +153,6 @@ class Servo
    */
   void setSmoothingPlugin();
 
-  /**
-   * \brief The callback for velocity scaling values from collision checker.
-   */
-  void collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg);
-
   /**
    * \brief Updates the servo parameters and performs some validations.
    */
@@ -181,9 +184,7 @@ class Servo
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
   std::atomic<double> collision_velocity_scale_ = 1.0;
-  std::unique_ptr<CollisionCheck> collision_checker_;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr collision_velocity_scale_sub_;
-
+  std::unique_ptr<CollisionMonitor> collision_monitor_;
   pluginlib::UniquePtr<online_signal_smoothing::SmoothingBaseClass> smoother_;
 
   size_t num_joints_;