Support nonholonomic movement for slotcar (#33)

* nonholonomic car path following Signed-off-by: ddengster <[email protected]> * add nonholonomic path request subscriber with mutex locks, plus some cleanup Signed-off-by: ddengster <[email protected]> * fix issue with subsequent ackmann path requests attempted fix on turning for straight line ackmann paths more cleanup Signed-off-by: ddengster <[email protected]> * uncrustify Signed-off-by: ddengster <[email protected]> * change back to private, add is_holonomic function Signed-off-by: ddengster <[email protected]> * compile fix Signed-off-by: ddengster <[email protected]> * some cleanup Signed-off-by: ddengster <[email protected]> * style Signed-off-by: ddengster <[email protected]> * undo divide by dt remove snap_world_pose code Signed-off-by: ddengster <[email protected]> * add min_turning_radius tag account for robot names in ackmann_path_request remove unneeded world pose condition Signed-off-by: ddengster <[email protected]> * lots of code removal add condition for ackmann_path_request subscription Signed-off-by: ddengster <[email protected]> * add comments Signed-off-by: ddengster <[email protected]> * style Signed-off-by: ddengster <[email protected]> * update function params Signed-off-by: ddengster <[email protected]> * update again Signed-off-by: ddengster <[email protected]> Co-authored-by: Luca Della Vedova <[email protected]>
open-rmf · Aug 17, 2021 · a80cb6e · a80cb6e
1 parent 0455e7a
commit a80cb6e
Show file tree

Hide file tree

Showing 3 changed files with 345 additions and 8 deletions.
diff --git a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
@@ -36,6 +36,31 @@ namespace rmf_robot_sim_common {
 
 // TODO migrate ign-math-eigen conversions when upgrading to ign-math5
 
+//3rd coordinate is yaw
+struct NonHolonomicTrajectory
+{
+  NonHolonomicTrajectory(const Eigen::Vector2d& _x0, const Eigen::Vector2d& _x1,
+    const Eigen::Vector2d& _v1 = Eigen::Vector2d(0, 0),
+    bool _turning = false)
+  : x0(_x0), x1(_x1),
+    v0((x1 - x0).normalized()), v1(_v1),
+    turning(_turning)
+  {}
+  // positions
+  Eigen::Vector2d x0;
+  Eigen::Vector2d x1;
+  // headings
+  Eigen::Vector2d v0;
+  Eigen::Vector2d v1;
+
+  bool turning = false;
+  float turning_radius = 0.0f;
+  float turn_arc_radians = 0.0f;
+  float turn_arclength = 0.0f;
+  Eigen::Vector2d turn_circle_center;
+  Eigen::Vector2d turn_target_heading;
+};
+
 // Edit reference of parameter for template type deduction
 template<typename IgnQuatT>
 inline void convert(const Eigen::Quaterniond& _q, IgnQuatT& quat)
@@ -120,6 +145,8 @@ class SlotcarCommon
     const std::vector<Eigen::Vector3d>& obstacle_positions,
     const double time);
 
+  std::pair<double, double> update_nonholonomic(Eigen::Isometry3d& pose);
+
   bool emergency_stop(const std::vector<Eigen::Vector3d>& obstacle_positions,
     const Eigen::Vector3d& current_heading);
 
@@ -142,6 +169,8 @@ class SlotcarCommon
 
   void publish_robot_state(const double time);
 
+  bool is_holonomic();
+
 private:
   // Paramters needed for power dissipation and charging calculations
   // Default values may be overriden using values from sdf file
@@ -181,13 +210,16 @@ class SlotcarCommon
   std::size_t _sequence = 0;
 
   std::vector<Eigen::Isometry3d> trajectory;
-  std::size_t _traj_wp_idx;
+  std::size_t _traj_wp_idx = 0;
+  std::vector<NonHolonomicTrajectory> nonholonomic_trajectory;
+  std::size_t _nonholonomic_traj_idx = 0;
 
   rmf_fleet_msgs::msg::PauseRequest pause_request;
 
   std::vector<rclcpp::Time> _hold_times;
 
   std::mutex _mutex;
+  std::mutex _ackmann_path_req_mutex;
 
   std::string _model_name;
   bool _emergency_stop = false;
@@ -214,6 +246,7 @@ class SlotcarCommon
     _building_map_sub;
 
   rmf_fleet_msgs::msg::RobotMode _current_mode;
+  bool _is_holonomic = true;
 
   std::string _current_task_id;
   std::vector<rmf_fleet_msgs::msg::Location> _remaining_path;
@@ -237,6 +270,8 @@ class SlotcarCommon
   double _stop_distance = 1.0;
   double _stop_radius = 1.0;
 
+  double _min_turning_radius = -1.0; // minimum turning radius, will use a formula if negative
+
   PowerParams _params;
   bool _enable_charge = true;
   bool _enable_instant_charge = false;
@@ -271,6 +306,9 @@ class SlotcarCommon
 
   void path_request_cb(const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
 
+  void ackmann_path_request_cb(
+    const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
+
   void pause_request_cb(const rmf_fleet_msgs::msg::PauseRequest::SharedPtr msg);
 
   void mode_request_cb(const rmf_fleet_msgs::msg::ModeRequest::SharedPtr msg);
@@ -304,6 +342,12 @@ bool get_element_val_if_present(
 template<typename SdfPtrT>
 void SlotcarCommon::read_sdf(SdfPtrT& sdf)
 {
+  get_element_val_if_present<SdfPtrT, bool>(sdf, "holonomic",
+    this->_is_holonomic);
+  RCLCPP_INFO(
+    logger(),
+    "Vehicle is %s", _is_holonomic ? "holonomic" : "non-holonomic");
+
   get_element_val_if_present<SdfPtrT, double>(sdf, "nominal_drive_speed",
     this->_nominal_drive_speed);
   RCLCPP_INFO(
@@ -346,6 +390,13 @@ void SlotcarCommon::read_sdf(SdfPtrT& sdf)
     "Setting max turn acceleration to: %f",
     _max_turn_acceleration);
 
+  get_element_val_if_present<SdfPtrT, double>(sdf,
+    "min_turning_radius", this->_min_turning_radius);
+  RCLCPP_INFO(
+    logger(),
+    "Setting minimum turning radius to: %f",
+    _min_turning_radius);
+
   get_element_val_if_present<SdfPtrT, double>(sdf,
     "stop_distance", this->_stop_distance);
   RCLCPP_INFO(logger(), "Setting stop distance to: %f", _stop_distance);