Merge branch 'main' into fix/controller-is-too-conservative-in-its-co…

…llision-checking

CMakeLists.txt

@@ -12,7 +12,6 @@ find_package(catkin REQUIRED
     mbf_costmap_core
     mbf_msgs
     message_generation
-    nav_core
     nav_msgs
     pluginlib
     roscpp

include/path_tracking_pid/controller.hpp

@@ -66,17 +66,18 @@ class Controller : private boost::noncopyable
    * @param estimate_pose_angle The transformation from base to steered wheel
    */
   void setTricycleModel(
-    bool tricycle_model_enabled, const geometry_msgs::Transform & tf_base_to_steered_wheel);
+    bool tricycle_model_enabled, const tf2::Transform & tf_base_to_steered_wheel);
 
   /**
    * Set plan
    * @param current Where is the robot now?
    * @param odom_twist Robot odometry
    * @param global_plan Plan to follow
+   * @return whether the plan was successfully updated or not
    */
-  void setPlan(
-    const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
-    const std::vector<geometry_msgs::PoseStamped> & global_plan);
+  bool setPlan(
+    const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
+    const std::vector<tf2::Transform> & global_plan);
 
   /**
    * Set plan
@@ -85,12 +86,14 @@ class Controller : private boost::noncopyable
    * @param tf_base_to_steered_wheel Where is the steered wheel now?
    * @param steering_odom_twist Steered wheel odometry
    * @param global_plan Plan to follow
+   * @return whether the plan was successfully updated or not
    */
-  void setPlan(
-    const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
-    const geometry_msgs::Transform & tf_base_to_steered_wheel,
+  bool setPlan(
+    const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
+    const tf2::Transform & tf_base_to_steered_wheel,
     const geometry_msgs::Twist & steering_odom_twist,
-    const std::vector<geometry_msgs::PoseStamped> & global_plan);
+    const std::vector<tf2::Transform> & global_plan);
+
   /**
    * Find position on plan by looking at the surroundings of last known pose.
    * @param current Where is the robot now?
@@ -99,11 +102,10 @@ class Controller : private boost::noncopyable
    * @return index of current path-pose if requested
    */
   tf2::Transform findPositionOnPlan(
-    const geometry_msgs::Transform & current_tf, ControllerState & controller_state,
-    size_t & path_pose_idx);
+    const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx);
   // Overloaded function definition for users that don't require the segment index
   tf2::Transform findPositionOnPlan(
-    const geometry_msgs::Transform & current_tf, ControllerState & controller_state)
+    const tf2::Transform & current_tf, ControllerState & controller_state)
   {
     size_t path_pose_idx;
     return findPositionOnPlan(current_tf, controller_state, path_pose_idx);
@@ -127,7 +129,7 @@ class Controller : private boost::noncopyable
    * @return Update result
    */
   UpdateResult update(
-    double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf,
+    double target_x_vel, double target_end_x_vel, const tf2::Transform & current_tf,
     const geometry_msgs::Twist & odom_twist, ros::Duration dt);
 
   /**
@@ -138,8 +140,7 @@ class Controller : private boost::noncopyable
    * @return Update result
    */
   UpdateResult update_with_limits(
-    const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
-    ros::Duration dt);
+    const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt);
 
   /**
    * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds
@@ -148,7 +149,7 @@ class Controller : private boost::noncopyable
    * @return Velocity command
    */
   double mpc_based_max_vel(
-    double target_x_vel, const geometry_msgs::Transform & current_tf,
+    double target_x_vel, const tf2::Transform & current_tf,
     const geometry_msgs::Twist & odom_twist);
 
   /**
@@ -250,7 +251,7 @@ class Controller : private boost::noncopyable
 
   // tricycle model
   bool use_tricycle_model_ = false;
-  geometry_msgs::Transform tf_base_to_steered_wheel_;
+  tf2::Transform tf_base_to_steered_wheel_;
   std::array<std::array<double, 2>, 2> inverse_kinematics_matrix_{};
   std::array<std::array<double, 2>, 2> forward_kinematics_matrix_{};
 

include/path_tracking_pid/path_tracking_pid_local_planner.hpp

@@ -4,7 +4,6 @@
 #include <dynamic_reconfigure/server.h>
 #include <geometry_msgs/Point.h>
 #include <mbf_costmap_core/costmap_controller.h>
-#include <nav_core/base_local_planner.h>
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/Path.h>
 #include <path_tracking_pid/PidConfig.h>
@@ -26,8 +25,7 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x,
 
 namespace path_tracking_pid
 {
-class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
-                                public mbf_costmap_core::CostmapController,
+class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
                                 private boost::noncopyable
 {
 private:
@@ -57,14 +55,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
     */
   bool setPlan(const std::vector<geometry_msgs::PoseStamped> & global_plan) override;
 
-  /**
-   * @brief Calculates the velocity command based on the current robot pose given by pose. See the interface in move
-   * base.
-   * @param cmd_vel Output the velocity command
-   * @return true if succeded.
-   */
-  bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel) override;  // NOLINT
-
   /**
    * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity
    * and message are not set. See the interface in move base flex.
@@ -76,13 +66,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
    */
   uint32_t computeVelocityCommands(
     const geometry_msgs::PoseStamped & pose, const geometry_msgs::TwistStamped & velocity,
-    geometry_msgs::TwistStamped & cmd_vel, std::string & message) override;  // NOLINT
-
-  /**
-   * @brief Returns true, if the goal is reached. Currently does not respect the parameters give
-   * @return true, if the goal is reached
-   */
-  bool isGoalReached() override;
+    geometry_msgs::TwistStamped & cmd_vel, std::string & message) override;
 
   /**
    * @brief Returns true, if the goal is reached. Currently does not respect the parameters given.
@@ -104,6 +88,18 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
   enum class ComputeVelocityCommandsResult { SUCCESS = 0, GRACEFULLY_CANCELLING = 1 };
 
 private:
+  /**
+   * @brief Calculates the velocity command based on the current robot pose given by pose.
+   * @return Velocity command on success, empty optional otherwise.
+   */
+  std::optional<geometry_msgs::Twist> computeVelocityCommands();
+
+  /**
+   * @brief Returns true, if the goal is reached.
+   * @return true, if the goal is reached
+   */
+  bool isGoalReached() const;
+
   /**
    * Accept a new configuration for the PID controller
    * @param config the new configuration
@@ -123,9 +119,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
   ros::Duration prev_dt_;
   path_tracking_pid::Controller pid_controller_;
 
-  std::vector<geometry_msgs::PoseStamped> global_plan_;
-  nav_msgs::Path received_path_;
-
   // Obstacle collision detection
   costmap_2d::Costmap2DROS * costmap_ = nullptr;
 
@@ -146,7 +139,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
   // Rviz visualization
   std::unique_ptr<Visualization> visualization_;
   ros::Publisher path_pub_;
-  ros::Publisher collision_marker_pub_;
 
   ros::Subscriber sub_odom_;
   ros::Publisher feedback_pub_;

include/path_tracking_pid/visualization.hpp

@@ -4,27 +4,39 @@
 #include <tf2/LinearMath/Transform.h>
 
 #include <string>
+#include <vector>
 
 namespace path_tracking_pid
 {
 class Visualization
 {
 public:
-  Visualization(ros::NodeHandle nh);
+  explicit Visualization(ros::NodeHandle nh);
 
   void publishControlPoint(const std_msgs::Header & header, const tf2::Transform & pose);
   void publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose);
   void publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose);
   void publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose);
+  void publishCollisionObject(
+    const std_msgs::Header & header, uint8_t cost, const tf2::Vector3 & point);
+  void publishExtrapolatedPoses(
+    const std_msgs::Header & header, const std::vector<tf2::Vector3> & steps);
+  void publishgGoalPosesOnPath(
+    const std_msgs::Header & header, const std::vector<tf2::Vector3> & path);
+  void publishCollisionFootprint(
+    const std_msgs::Header & header, const std::vector<tf2::Vector3> & footprint);
+  void publishCollisionPolygon(
+    const std_msgs::Header & header, const std::vector<tf2::Vector3> & hull);
 
 private:
+  ros::Publisher marker_pub_;
+
   void publishSphere(
-    const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose,
+    const std_msgs::Header & header, const std::string & ns, const tf2::Transform & pose,
     const std_msgs::ColorRGBA & color);
   void publishSphere(
-    const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose,
+    const std_msgs::Header & header, const std::string & ns, const geometry_msgs::Pose & pose,
     const std_msgs::ColorRGBA & color);
-
-  ros::Publisher marker_pub_;
 };
+
 }  // namespace path_tracking_pid

package.xml

@@ -22,7 +22,6 @@
   <depend>geometry_msgs</depend>
   <depend>mbf_costmap_core</depend>
   <depend>mbf_msgs</depend>
-  <depend>nav_core</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
@@ -37,7 +36,6 @@
   <test_depend>rospy</test_depend>
 
   <export>
-    <nav_core plugin="${prefix}/path_tracking_pid_plugin.xml"/>
     <mbf_costmap_core plugin="${prefix}/path_tracking_pid_plugin.xml"/>
   </export>
 </package>

path_tracking_pid_plugin.xml

@@ -1,9 +1,4 @@
 <library path="lib/libpath_tracking_pid">
-    <class name="path_tracking_pid/TrackingPidLocalPlanner" type="path_tracking_pid::TrackingPidLocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
-        <description>
-            Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity
-        </description>
-    </class>
     <class name="path_tracking_pid/TrackingPidLocalPlanner" type="path_tracking_pid::TrackingPidLocalPlanner" base_class_type="mbf_costmap_core::CostmapController">
         <description>
             Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity

src/calculations.cpp

@@ -62,4 +62,13 @@ std::vector<double> inverse_turning_radiuses(const std::vector<tf2::Transform> &
   return result;
 }
 
+bool is_in_direction_of_target(
+  const tf2::Transform & current, const tf2::Vector3 & target, double velocity)
+{
+  const auto delta = target - current.getOrigin();
+  const auto projection = current.getBasis().tdotx(delta);
+
+  return !std::signbit(projection * velocity);
+}
+
 }  // namespace path_tracking_pid

src/calculations.hpp

@@ -34,4 +34,16 @@ std::vector<double> distances_to_goal(const std::vector<tf2::Transform> & deltas
  */
 std::vector<double> inverse_turning_radiuses(const std::vector<tf2::Transform> & deltas);
 
+/**
+ * Determine if the given current pose is in the direction of the given target position taking the
+ * given velocity into account.
+ *
+ * @param[in] current Current pose.
+ * @param[in] target Target position.
+ * @param[in] velocity Forward velocity in m/s.
+ * @return True if it is in the direction of the target, false otherwise.
+ */
+bool is_in_direction_of_target(
+  const tf2::Transform & current, const tf2::Vector3 & target, double velocity);
+
 }  // namespace path_tracking_pid

src/common.hpp

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <tf2/convert.h>
+
 #include <type_traits>
 
 namespace path_tracking_pid
@@ -13,4 +15,23 @@ constexpr std::underlying_type_t<enum_type> to_underlying(enum_type value) noexc
   return static_cast<std::underlying_type_t<enum_type>>(value);
 }
 
+/**
+ * Converts input (of input_type) to result_type. Like tf2::fromMsg() but using a return value
+ * instead of an output parameter.
+ * 
+ * @tparam result_type Resulting type of the conversion. Should be a tf2 type.
+ * @tparam input_type Input type for the conversion. Should be a message type.
+ * @param[in] input Input object to convert.
+ * @return Converted object.
+ */
+template <
+  typename result_type, typename input_type,
+  typename = std::enable_if_t<!std::is_same_v<result_type, input_type>>>
+result_type tf2_convert(const input_type & input)
+{
+  result_type result;
+  tf2::fromMsg(input, result);
+  return result;
+}
+
 }  // namespace path_tracking_pid