Merge branch 'main' into feature/ignition_rosdep_key

.github/workflows/build.yaml

@@ -9,15 +9,20 @@ jobs:
   build_and_test:
     name: build_and_test
     runs-on: ubuntu-20.04
+    strategy:
+      matrix:
+        ros_distribution:
+          - foxy
+          - galactic
     steps:
       - name: deps
         uses: ros-tooling/[email protected]
         with:
-          required-ros-distributions: foxy
+          required-ros-distributions: ${{ matrix.ros_distribution }}
       - name: build_and_test
         uses: ros-tooling/[email protected]
         with:
-          target-ros2-distro: foxy
+          target-ros2-distro: ${{ matrix.ros_distribution }}
           # build all packages listed in the meta package
           package-name: |
             rmf_building_sim_common

rmf_building_sim_ignition_plugins/src/crowd_simulator.cpp

@@ -92,6 +92,10 @@ void CrowdSimulatorPlugin::PreUpdate(
     return;
   }
 
+  // Don't update the plugin if the simulation is paused
+  if (info.paused)
+    return;
+
   // Note, the update_time_step parameter is ignored in ignition
   // through GPU animated actors the performance is good enough that
   // we can afford to update at every iteration and have smooth animations

rmf_building_sim_ignition_plugins/src/door.cpp

@@ -114,6 +114,10 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin
     if (!_initialized)
       return;
 
+    // Don't update the pose if the simulation is paused
+    if (info.paused)
+      return;
+
     double t =
       (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).
       count()) * 1e-9;

rmf_building_sim_ignition_plugins/src/lift.cpp

@@ -255,6 +255,10 @@ class IGNITION_GAZEBO_VISIBLE LiftPlugin
     if (!_initialized)
       return;
 
+    // Don't update the pose if the simulation is paused
+    if (info.paused)
+      return;
+
     // Send update request
     const double t =
       (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).

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);