Feature/waypoint controller base

hector_ros_controllers/CMakeLists.txt

@@ -32,6 +32,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     std_msgs
     std_srvs
     fcl
+    tf2_ros
+    tf2_geometry_msgs
+    hector_ros_controllers_msgs
+    lifecycle_msgs
     ad_kinematics)
 
 foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -53,12 +57,17 @@ generate_parameter_library(
 generate_parameter_library(safety_forward_controller_parameters
                            params/safety_forward_controller_parameters.yaml)
 
+generate_parameter_library(waypoint_controller_base_parameters
+                           params/waypoint_controller_base_parameters.yaml)
+
 generate_parameter_library(safety_position_controller_parameters
                            params/safety_position_controller_parameters.yaml)
-# Add controllers here Define Controllers as Shared Libraries
 
+# Add controllers here Define Controllers as Shared Libraries
 add_library(
   controllers SHARED
+  src/simple_waypoint_controller.cpp
+  src/waypoint_controller_base.cpp
   src/velocity_to_position_controllers_base.cpp
   src/safety_forward_controller.cpp
   src/velocity_to_position_command_controller.cpp
@@ -80,7 +89,8 @@ target_link_libraries(
          velocity_to_position_command_controller_parameters
          self_collision_avoidance_controller_parameters
          safety_forward_controller_parameters
-         safety_position_controller_parameters)
+         safety_position_controller_parameters
+         waypoint_controller_base_parameters)
 
 target_link_libraries(controllers PUBLIC ad_kinematics::ad_kinematics)
 ament_target_dependencies(controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -97,6 +107,7 @@ install(
           self_collision_avoidance_controller_parameters
           safety_forward_controller_parameters
           safety_position_controller_parameters
+          waypoint_controller_base_parameters
   EXPORT export_hector_ros_controllers
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib

hector_ros_controllers/hector_ros_controller_plugin.xml

@@ -21,4 +21,9 @@
             A chainable position controller ensuring that revolute joints are unwrapped and joint limits are satisfied.
         </description>
   </class>
+  <class name="waypoint_controller/SimpleWaypointController" type="waypoint_controller::SimpleWaypointController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+            A simple waypoint controller that calculates velocity commands to navigate a robot to a given waypoint.
+        </description>
+  </class>
 </library>

hector_ros_controllers/include/waypoint_controller/simple_waypoint_controller.hpp

@@ -0,0 +1,16 @@
+#ifndef SIMPLE_WAYPOINT_CONTROLLER__SIMPLE_WAYPOINT_CONTROLLER_HPP_
+#define SIMPLE_WAYPOINT_CONTROLLER__SIMPLE_WAYPOINT_CONTROLLER_HPP_
+
+#include "waypoint_controller/waypoint_controller_base.hpp"
+
+namespace waypoint_controller
+{
+class SimpleWaypointController : public waypoint_controller::WaypointControllerBase
+{
+  MoveCommand computeCommand( const Waypoint &goal, const Pose &pose, const double &curr_linear_vel,
+                              const double &curr_angular_vel, const bool &is_final_goal ) override;
+};
+
+} // namespace waypoint_controller
+
+#endif // HPP

hector_ros_controllers/include/waypoint_controller/waypoint_controller_base.hpp

@@ -0,0 +1,182 @@
+#ifndef WAYPOINT_CONTROLLER__WAYPOINT_CONTROLLER_BASE_HPP_
+#define WAYPOINT_CONTROLLER__WAYPOINT_CONTROLLER_BASE_HPP_
+
+#include <boost/shared_ptr.hpp>
+#include <float.h>
+#include <memory>
+#include <string>
+#include <urdf_parser/urdf_parser.h>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_double_buffer.hpp"
+#include "realtime_tools/realtime_server_goal_handle.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "tf2/exceptions.h"
+#include "tf2/utils.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include <hector_ros_controllers_msgs/action/waypoint_navigation.hpp>
+
+// auto-generated by generate_parameter_library
+#include <hector_ros_controllers/waypoint_controller_base_parameters.hpp>
+
+namespace waypoint_controller
+{
+
+struct Waypoint {
+  Waypoint()
+  {
+    x = 0.0;
+    y = 0.0;
+    heading = 0.0;
+  }
+  Waypoint( const geometry_msgs::msg::Point &point, const double &head ) : heading( head )
+  {
+    x = point.x;
+    y = point.y;
+  }
+
+  double x;
+  double y;
+  double heading;
+};
+
+struct MoveCommand {
+  double linear_vel_cmd;
+  double angual_vel_cmd;
+};
+
+struct Pose {
+  Pose() { }
+  Pose( double x_val, double y_val, double heading_val )
+      : x( x_val ), y( y_val ), heading( heading_val )
+  {
+  }
+  double x;
+  double y;
+  double heading;
+};
+
+using WaypointNav = typename hector_ros_controllers_msgs::action::WaypointNavigation;
+using RtGhWayNav =
+    realtime_tools::RealtimeServerGoalHandle<hector_ros_controllers_msgs::action::WaypointNavigation>;
+
+class WaypointControllerBase : public controller_interface::ControllerInterface
+{
+public:
+  WaypointControllerBase();
+
+  ~WaypointControllerBase() = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::CallbackReturn
+  on_configure( const rclcpp_lifecycle::State &previous_state ) override;
+
+  controller_interface::CallbackReturn
+  on_activate( const rclcpp_lifecycle::State &previous_state ) override;
+
+  controller_interface::CallbackReturn
+  on_deactivate( const rclcpp_lifecycle::State &previous_state ) override;
+
+  rclcpp_action::CancelResponse goal_cancelled_callback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<WaypointNav>> goal_handle );
+
+  rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID &,
+                                                      std::shared_ptr<const WaypointNav::Goal> goal );
+
+  void
+  goal_accepted_callback( std::shared_ptr<rclcpp_action::ServerGoalHandle<WaypointNav>> goal_handle );
+
+  controller_interface::return_type update( const rclcpp::Time &time,
+                                            const rclcpp::Duration &period ) override;
+
+protected:
+  virtual MoveCommand computeCommand( const Waypoint &goal, const Pose &pose,
+                                      const double &curr_linear_vel, const double &curr_angular_vel,
+                                      const bool &is_final_goal ) = 0;
+
+  virtual bool check_goal_completion( const Waypoint &goal, const Pose &pose, bool is_final_goal );
+
+  controller_interface::CallbackReturn read_parameters();
+  void declare_parameters();
+
+private:
+  bool validate_trajectory( const std::vector<geometry_msgs::msg::Point> &point_trajectory,
+                            const std::vector<double> &heading_trajectory );
+
+  void update_pose_cb();
+
+  bool set_base_velocities( const MoveCommand &cmd );
+  bool stop_base();
+
+  void preempt_active_goal( std::shared_ptr<RtGhWayNav> active_trajectory );
+
+  void update_feedback();
+  WaypointNav::Result get_result_msg( bool success, const std::string &failure_report );
+
+  // Parameters
+  bool use_cmd_vel_;
+  std::chrono::milliseconds action_monitor_period_;
+  std::string velocity_interfaces_prefix_;
+  std::string base_link_frame_;
+  std::string pose_reference_frame_;
+  double point_completion_tolerance_;
+  double heading_completion_tolerance_;
+
+  // Interfaces
+  std::vector<std::string> command_interface_names_;
+  std::vector<std::string> state_interface_names_;
+
+  rclcpp_action::Server<hector_ros_controllers_msgs::action::WaypointNavigation>::SharedPtr navigation_server_;
+
+  // Updated via Non-RT thread
+  realtime_tools::RealtimeBuffer<std::shared_ptr<const WaypointNav::Goal>> trajectory_buffer_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<RtGhWayNav>> trajectory_gh_buffer_;
+
+  // Accessed only by RT thread
+  std::shared_ptr<const WaypointNav::Goal> active_trajectory_;
+  std::shared_ptr<RtGhWayNav> active_gh_;
+
+  std::shared_ptr<waypoint_controller_base_parameters::ParamListener> base_param_listener_;
+  waypoint_controller_base_parameters::Params base_params_;
+
+  // canceled_is only managed by Non-RT thread, active_ only by RT thread
+  // This prevents logical race conditions
+  std::atomic<bool> canceled_;
+  bool active_;
+
+  std::atomic<bool> reset_trajectory_;
+  size_t current_goal_idx_;
+
+  Waypoint current_goal_;
+  MoveCommand current_cmd_;
+  realtime_tools::RealtimeBuffer<Pose> current_pose_;
+  realtime_tools::RealtimeDoubleBuffer<WaypointNav::Feedback> feedback_buffer_;
+
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>> vel_pub_{ nullptr };
+
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{ nullptr };
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  rclcpp::TimerBase::SharedPtr pose_update_timer_{ nullptr };
+
+  // Monitors action goal status
+  rclcpp::TimerBase::SharedPtr action_monitor_timer_{ nullptr };
+};
+
+} // namespace waypoint_controller
+
+#endif // HPP

hector_ros_controllers/package.xml

@@ -16,8 +16,10 @@
   <depend>controller_interface</depend>
   <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>
+  <depend>hector_ros_controllers_msgs</depend>
   <depend>hpp-fcl</depend>
   <depend>libfcl-dev</depend>
+  <depend>lifecycle_msgs</depend>
   <depend>pinocchio</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
@@ -26,6 +28,8 @@
   <depend>srdfdom</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>visualization_msgs</depend>
 
   <export>

hector_ros_controllers/params/waypoint_controller_base_parameters.yaml

@@ -0,0 +1,17 @@
+waypoint_controller_base_parameters:
+
+  use_cmd_vel: {type: bool, default_value: false, description: 'Instead of claiming robot interfaces, vel commands are published to robot/cmd_vel. Easier to use within simulation context. Should be used with caution on real robot since additional velocity commands are not blocked.'}
+
+  velocity_interfaces_prefix: {type: string, default_value: '', description: Prefix of base velocity interfaces.}
+
+  base_link_frame_name: {type: string, default_value: base_link, description: The name of the robot's base link frame.}
+
+  pose_reference_frame_name: {type: string, default_value: map, description: The name of the reference frame for the robot's pose.}
+
+  action_monitior_period: {type: int, default_value: 10, description: Time period in milli seconds for monitoring the action goal status.}
+
+  point_completion_tolerance: {type: double, default_value: 0.1, description: Distance tolerance in meters to consider a waypoint goal as reached.}
+
+  heading_completion_tolerance: {type: double, default_value: 0.1, description: Heading tolerance in radians to consider a waypoint goal as reached.}
+
+  e_stop_topic: {type: string, default_value: estop_board/hard_estop, description: 'Topic to subscribe to for emergency stop messages. If set, the controller will stop all commands when an emergency stop message is received.'}

hector_ros_controllers/src/simple_waypoint_controller.cpp

@@ -0,0 +1,43 @@
+#include "waypoint_controller/simple_waypoint_controller.hpp"
+namespace waypoint_controller
+{
+MoveCommand SimpleWaypointController::computeCommand( const Waypoint &goal, const Pose &pose,
+                                                      const double &curr_linear_vel,
+                                                      const double &curr_angular_vel,
+                                                      const bool &is_final_goal )
+{
+  MoveCommand cmd;
+
+  // Gains (tune these)
+  const double k_rho = 0.8;
+  const double k_alpha = 2.0;
+
+  // Velocity limits
+  const double v_max = 0.5;     // m/s
+  const double omega_max = 1.5; // rad/s
+
+  // Position error
+  double dx = goal.x - pose.x;
+  double dy = goal.y - pose.y;
+
+  // Distance to goal
+  double rho = std::sqrt( dx * dx + dy * dy );
+
+  // Desired heading
+  double theta_des = std::atan2( dy, dx );
+
+  // Heading error
+  double alpha = ( theta_des - pose.heading );
+  alpha = std::atan2( std::sin( alpha ), std::cos( alpha ) );
+
+  // Control law
+  cmd.linear_vel_cmd = std::clamp( k_rho * rho, -v_max, v_max );
+  cmd.angual_vel_cmd = std::clamp( k_alpha * alpha, -omega_max, omega_max );
+
+  return cmd;
+}
+} // namespace waypoint_controller
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS( waypoint_controller::SimpleWaypointController,
+                        controller_interface::ControllerInterface )