Pure pursuit ackermann vehicles controller

rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp

@@ -34,32 +34,16 @@
 
 namespace rmf_robot_sim_common {
 
-//3rd coordinate is yaw
-struct AckermannTrajectory
+struct SlotcarTrajectory
 {
-  AckermannTrajectory(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)
+  SlotcarTrajectory(const Eigen::Isometry3d& _pose)
+  : pose(_pose)
   {}
+  SlotcarTrajectory() {}
   // positions
-  Eigen::Vector2d x0;
-  Eigen::Vector2d x1;
-  // headings
-  Eigen::Vector2d v0;
-  Eigen::Vector2d v1;
-
-  bool turning = false;
-};
-
-struct TrajectoryPoint
-{
-  Eigen::Vector3d pos;
-  Eigen::Quaterniond quat;
-  TrajectoryPoint(const Eigen::Vector3d& _pos, const Eigen::Quaterniond& _quat)
-  : pos(_pos), quat(_quat) {}
+  Eigen::Isometry3d pose;
+  // Maximum speed for the lane approaching this waypoint
+  std::optional<double> approach_speed_limit;
 };
 
 // steering type constants
@@ -76,7 +60,9 @@ class SlotcarCommon
   {
     double v = 0.0; // Target displacement in X (forward)
     double w = 0.0; // Target displacement in yaw
-    double speed = 0.0; // Target speed
+    double target_linear_speed_now = 0.0;     // Target linear speed now
+    double target_linear_speed_destination = 0.0; // Target linear speed at destination
+    std::optional<double> max_speed = std::nullopt; // Maximum speed allowed while navigating
   };
 
   SlotcarCommon();
@@ -103,19 +89,33 @@ class SlotcarCommon
     2>& curr_velocities,
     const std::pair<double, double>& displacements,
     const double dt,
-    const double target_linear_velocity = 0.0) const;
+    const double target_velocity_now = 0.0,
+    const double target_velocity_at_dest = 0.0,
+    const std::optional<double>& linear_speed_limit = std::nullopt) const;
 
   std::array<double, 2> calculate_joint_control_signals(
     const std::array<double, 2>& w_tire,
     const std::pair<double, double>& displacements,
-    const double dt) const;
+    const double dt,
+    const double target_linear_speed_now = 0.0,
+    const double target_linear_speed_destination = 0.0,
+    const std::optional<double>& linear_speed_limit = std::nullopt) const;
 
   void charge_state_cb(const std::string& name, bool selected);
 
   void publish_robot_state(const double time);
 
+  Eigen::Vector3d get_lookahead_point() const;
+
+  bool display_markers = false; // Ignition only: toggles display of waypoint and lookahead markers
+
+  using PathRequestCallback =
+    std::function<void(const rmf_fleet_msgs::msg::PathRequest::SharedPtr)>;
+  void set_path_request_callback(PathRequestCallback cb)
+  { _path_request_callback = cb; }
+
 private:
-  // Paramters needed for power dissipation and charging calculations
+  // Parameters needed for power dissipation and charging calculations
   // Default values may be overriden using values from sdf file
   struct PowerParams
   {
@@ -152,10 +152,8 @@ class SlotcarCommon
   double last_topic_pub = 0.0;
   std::size_t _sequence = 0;
 
-  std::vector<Eigen::Isometry3d> trajectory;
+  std::vector<SlotcarTrajectory> trajectory;
   std::size_t _traj_wp_idx = 0;
-  std::vector<AckermannTrajectory> ackermann_trajectory;
-  std::size_t _ackermann_traj_idx = 0;
 
   rmf_fleet_msgs::msg::PauseRequest pause_request;
 
@@ -172,6 +170,7 @@ class SlotcarCommon
   // Assumes robot is stationary upon initialization
   Eigen::Vector3d _old_lin_vel = Eigen::Vector3d::Zero(); // Linear velocity at previous time step
   double _old_ang_vel = 0.0; // Angular velocity at previous time step
+  bool _was_rotating; // Whether robot was rotating towards its next target in previous time step
   Eigen::Isometry3d _pose; // Pose at current time step
   int _rot_dir = 1; // Current direction of rotation
 
@@ -236,6 +235,11 @@ class SlotcarCommon
 
   bool _docking = false;
 
+  Eigen::Vector3d _lookahead_point;
+  double _lookahead_distance = 8.0;
+
+  PathRequestCallback _path_request_callback = nullptr;
+
   std::string get_level_name(const double z);
 
   double compute_change_in_rotation(
@@ -253,12 +257,6 @@ class SlotcarCommon
 
   void path_request_cb(const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
 
-  void handle_diff_drive_path_request(
-    const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
-
-  void handle_ackermann_path_request(
-    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);
@@ -305,9 +303,14 @@ void SlotcarCommon::read_sdf(SdfPtrT& sdf)
     steering_type);
 
   if (steering_type == "ackermann")
+  {
     _steering_type = SteeringType::ACKERMANN;
+    _reversible = false;
+  }
   else if (steering_type == "diff_drive")
+  {
     _steering_type = SteeringType::DIFF_DRIVE;
+  }
 
   RCLCPP_INFO(
     logger(),
@@ -435,6 +438,17 @@ void SlotcarCommon::read_sdf(SdfPtrT& sdf)
     "Setting nominal power to: %f",
     _params.nominal_power);
 
+  get_element_val_if_present<SdfPtrT, double>(sdf,
+    "lookahead_distance", this->_lookahead_distance);
+  RCLCPP_INFO(
+    logger(),
+    "Setting lookahead distance to: %f",
+    _lookahead_distance);
+
+  get_element_val_if_present<SdfPtrT, bool>(sdf,
+    "display_markers", this->display_markers);
+  RCLCPP_INFO(logger(), "Setting display_markers to: %d", display_markers);
+
   // Charger Waypoint coordinates are in child element of top level world element
   if (sdf->GetParent() && sdf->GetParent()->GetParent())
   {

rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp

@@ -52,16 +52,6 @@ struct SimEntity
 };
 
 /////////////////////////////////////////////////////////////////////////////////////////////////
-// TODO(MXG): Refactor the use of this function to replace it with
-// compute_desired_rate_of_change()
-double compute_ds(
-  double s_target,
-  double v_actual,
-  const double v_max,
-  const double accel_nom,
-  const double accel_max,
-  const double dt,
-  const double v_target = 0.0);
 
 struct MotionParams
 {
@@ -73,8 +63,10 @@ struct MotionParams
 };
 
 double compute_desired_rate_of_change(
-  double _s_target,
-  double _v_actual,
+  double _s_target,              // Displacement to destination
+  double _v_actual,              // Current velocity
+  double _speed_target_now,      // Target speed now while on route
+  double _speed_target_dest,     // Target speed at destination
   const MotionParams& _motion_params,
   const double _dt);