diff --git a/hector_ros_controllers/CMakeLists.txt b/hector_ros_controllers/CMakeLists.txt index 8c3a8d8..1dde4ea 100644 --- a/hector_ros_controllers/CMakeLists.txt +++ b/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 diff --git a/hector_ros_controllers/hector_ros_controller_plugin.xml b/hector_ros_controllers/hector_ros_controller_plugin.xml index 1b3de7f..130e74e 100644 --- a/hector_ros_controllers/hector_ros_controller_plugin.xml +++ b/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. + + + A simple waypoint controller that calculates velocity commands to navigate a robot to a given waypoint. + + diff --git a/hector_ros_controllers/include/waypoint_controller/simple_waypoint_controller.hpp b/hector_ros_controllers/include/waypoint_controller/simple_waypoint_controller.hpp new file mode 100644 index 0000000..dca37fc --- /dev/null +++ b/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 diff --git a/hector_ros_controllers/include/waypoint_controller/waypoint_controller_base.hpp b/hector_ros_controllers/include/waypoint_controller/waypoint_controller_base.hpp new file mode 100644 index 0000000..b29b6ce --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#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 + +// auto-generated by generate_parameter_library +#include + +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; + +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> goal_handle ); + + rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID &, + std::shared_ptr goal ); + + void + goal_accepted_callback( std::shared_ptr> 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 &point_trajectory, + const std::vector &heading_trajectory ); + + void update_pose_cb(); + + bool set_base_velocities( const MoveCommand &cmd ); + bool stop_base(); + + void preempt_active_goal( std::shared_ptr 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 command_interface_names_; + std::vector state_interface_names_; + + rclcpp_action::Server::SharedPtr navigation_server_; + + // Updated via Non-RT thread + realtime_tools::RealtimeBuffer> trajectory_buffer_; + realtime_tools::RealtimeBuffer> trajectory_gh_buffer_; + + // Accessed only by RT thread + std::shared_ptr active_trajectory_; + std::shared_ptr active_gh_; + + std::shared_ptr 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 canceled_; + bool active_; + + std::atomic reset_trajectory_; + size_t current_goal_idx_; + + Waypoint current_goal_; + MoveCommand current_cmd_; + realtime_tools::RealtimeBuffer current_pose_; + realtime_tools::RealtimeDoubleBuffer feedback_buffer_; + + std::shared_ptr> vel_pub_{ nullptr }; + + std::shared_ptr tf_listener_{ nullptr }; + std::unique_ptr 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 diff --git a/hector_ros_controllers/package.xml b/hector_ros_controllers/package.xml index e677102..2f5c5ec 100644 --- a/hector_ros_controllers/package.xml +++ b/hector_ros_controllers/package.xml @@ -16,8 +16,10 @@ controller_interface generate_parameter_library hardware_interface + hector_ros_controllers_msgs hpp-fcl libfcl-dev + lifecycle_msgs pinocchio pluginlib rclcpp @@ -26,6 +28,8 @@ srdfdom std_msgs std_srvs + tf2_geometry_msgs + tf2_ros visualization_msgs diff --git a/hector_ros_controllers/params/waypoint_controller_base_parameters.yaml b/hector_ros_controllers/params/waypoint_controller_base_parameters.yaml new file mode 100644 index 0000000..183e7f4 --- /dev/null +++ b/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.'} diff --git a/hector_ros_controllers/src/simple_waypoint_controller.cpp b/hector_ros_controllers/src/simple_waypoint_controller.cpp new file mode 100644 index 0000000..42cffd6 --- /dev/null +++ b/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 ) diff --git a/hector_ros_controllers/src/waypoint_controller_base.cpp b/hector_ros_controllers/src/waypoint_controller_base.cpp new file mode 100644 index 0000000..8d93d50 --- /dev/null +++ b/hector_ros_controllers/src/waypoint_controller_base.cpp @@ -0,0 +1,458 @@ +#include "waypoint_controller/waypoint_controller_base.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace waypoint_controller +{ +WaypointControllerBase::WaypointControllerBase() : controller_interface::ControllerInterface() { } + +controller_interface::CallbackReturn WaypointControllerBase::read_parameters() +{ + if ( !base_param_listener_ ) { + RCLCPP_ERROR( get_node()->get_logger(), "Error encountered during init" ); + return controller_interface::CallbackReturn::ERROR; + } + base_params_ = base_param_listener_->get_params(); + + if ( base_params_.base_link_frame_name.empty() ) { + RCLCPP_ERROR( get_node()->get_logger(), "Base link frame name cannot be empty" ); + return controller_interface::CallbackReturn::ERROR; + } + base_link_frame_ = base_params_.base_link_frame_name; + + if ( base_params_.pose_reference_frame_name.empty() ) { + RCLCPP_ERROR( get_node()->get_logger(), "Pose reference frame name cannot be empty" ); + return controller_interface::CallbackReturn::ERROR; + } + pose_reference_frame_ = base_params_.pose_reference_frame_name; + + try { + action_monitor_period_ = std::chrono::milliseconds( base_params_.action_monitior_period ); + } catch ( const std::exception &e ) { + RCLCPP_ERROR( get_node()->get_logger(), "Specified invalid action monitor period: %s", e.what() ); + } + + velocity_interfaces_prefix_ = base_params_.velocity_interfaces_prefix; + use_cmd_vel_ = base_params_.use_cmd_vel; + + if ( !use_cmd_vel_ ) { + // Define the command and state interface types based on the specified prefix + command_interface_names_.push_back( velocity_interfaces_prefix_ + "/linear/velocity" ); + command_interface_names_.push_back( velocity_interfaces_prefix_ + "/angular/velocity" ); + } + + if ( base_params_.point_completion_tolerance < 0 ) { + RCLCPP_ERROR( get_node()->get_logger(), "Point completion tolerance must be non-negative" ); + return controller_interface::CallbackReturn::ERROR; + } + point_completion_tolerance_ = base_params_.point_completion_tolerance; + + if ( base_params_.heading_completion_tolerance < 0 ) { + RCLCPP_ERROR( get_node()->get_logger(), "Heading completion tolerance must be non-negative" ); + return controller_interface::CallbackReturn::ERROR; + } + heading_completion_tolerance_ = base_params_.heading_completion_tolerance; + + return controller_interface::CallbackReturn::SUCCESS; +} + +void WaypointControllerBase::declare_parameters() +{ + base_param_listener_ = + std::make_shared( get_node() ); +} + +controller_interface::CallbackReturn WaypointControllerBase::on_init() +{ + try { + declare_parameters(); + + } catch ( const std::exception &e ) { + fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what() ); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void WaypointControllerBase::update_pose_cb() +{ + geometry_msgs::msg::TransformStamped t; + + try { + t = tf_buffer_->lookupTransform( pose_reference_frame_, base_link_frame_, tf2::TimePointZero ); + } catch ( const tf2::TransformException &ex ) { + RCLCPP_ERROR( get_node()->get_logger(), "Could not transform the base link frame to %s: %s", + pose_reference_frame_.c_str(), ex.what() ); + } + const auto pose = Pose( t.transform.translation.x, t.transform.translation.y, + tf2::getYaw( t.transform.rotation ) ); + current_pose_.writeFromNonRT( pose ); +} + +controller_interface::CallbackReturn +WaypointControllerBase::on_configure( const rclcpp_lifecycle::State & /*previous_state*/ ) +{ + auto ret = this->read_parameters(); + if ( ret != controller_interface::CallbackReturn::SUCCESS ) { + return ret; + } + + if ( use_cmd_vel_ ) { + vel_pub_ = get_node()->create_publisher( + std::string( get_node()->get_namespace() ) + "/cmd_vel", rclcpp::SystemDefaultsQoS() ); + } + + tf_buffer_ = std::make_unique( get_node()->get_clock() ); + tf_listener_ = std::make_shared( *tf_buffer_ ); + + pose_update_timer_ = get_node()->create_wall_timer( + std::chrono::milliseconds( 50 ), std::bind( &WaypointControllerBase::update_pose_cb, this ) ); + + pose_update_timer_->cancel(); + + navigation_server_ = rclcpp_action::create_server( + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string( get_node()->get_name() ) + "/waypoint_navigation", + std::bind( &WaypointControllerBase::goal_received_callback, this, std::placeholders::_1, + std::placeholders::_2 ), + std::bind( &WaypointControllerBase::goal_cancelled_callback, this, std::placeholders::_1 ), + std::bind( &WaypointControllerBase::goal_accepted_callback, this, std::placeholders::_1 ) ); + + RCLCPP_INFO( get_node()->get_logger(), "configure successful" ); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +WaypointControllerBase::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_names_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration WaypointControllerBase::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interface_config; + state_interface_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interface_config.names = state_interface_names_; + + return state_interface_config; +} + +controller_interface::CallbackReturn +WaypointControllerBase::on_activate( const rclcpp_lifecycle::State & /*previous_state*/ ) +{ + // check if we have all resources defined in the "points" parameter + // also verify that we *only* have the resources defined in the "points" parameter + std::vector> ordered_interfaces; + if ( !controller_interface::get_ordered_interfaces( command_interfaces_, command_interface_names_, + std::string( "" ), ordered_interfaces ) || + command_interface_names_.size() != ordered_interfaces.size() ) { + RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + command_interface_names_.size(), ordered_interfaces.size() ); + return controller_interface::CallbackReturn::ERROR; + } + + pose_update_timer_->reset(); + + // reset command buffer if a command came through callback when controller was inactive + trajectory_buffer_.writeFromNonRT( nullptr ); + + RCLCPP_INFO( get_node()->get_logger(), "activate successful" ); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +WaypointControllerBase::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/ ) +{ + canceled_.store( true ); + active_ = false; + + pose_update_timer_->cancel(); + + action_monitor_timer_->cancel(); + action_monitor_timer_ = nullptr; + + const auto active_trajectory = *trajectory_gh_buffer_.readFromNonRT(); + // Abort any currently active goal + if ( active_trajectory ) { + const auto action_res = get_result_msg( false, "Goal aborted due to controller deactivation" ); + active_trajectory->setAborted( std::make_shared( action_res ) ); + } + + trajectory_buffer_.writeFromNonRT( nullptr ); + trajectory_gh_buffer_.writeFromNonRT( std::shared_ptr() ); + + // Stop the robot + set_base_velocities( MoveCommand{ 0.0, 0.0 } ); + + return controller_interface::CallbackReturn::SUCCESS; +} + +bool WaypointControllerBase::check_goal_completion( const Waypoint &goal, const Pose ¤t_pose, + bool is_final_goal ) +{ + // Use euclidean distance to check if goal is reached + bool goal_reached = + std::sqrt( std::pow( goal.x - current_pose.x, 2 ) + std::pow( goal.y - current_pose.y, 2 ) ) <= + point_completion_tolerance_; + + if ( is_final_goal ) + goal_reached = goal_reached && ( std::abs( goal.heading - current_pose.heading ) <= + heading_completion_tolerance_ ); + + return goal_reached; +} + +rclcpp_action::CancelResponse WaypointControllerBase::goal_cancelled_callback( + const std::shared_ptr> goal_handle ) +{ + auto active_trajectory = *trajectory_gh_buffer_.readFromNonRT(); + + // Check that cancel request refers to currently active goal (if any) + if ( !canceled_.load() && active_trajectory->gh_ == goal_handle ) { + // Mark the current goal as canceled + canceled_.store( true ); + + const auto res_msg = get_result_msg( false, "Goal canceled by user callback" ); + active_trajectory->setCanceled( std::make_shared( res_msg ) ); + + trajectory_gh_buffer_.writeFromNonRT( std::shared_ptr() ); + trajectory_buffer_.writeFromNonRT( nullptr ); + + // action_monitor_timer_->cancel(); + // action_monitor_timer_ = nullptr; + + RCLCPP_INFO( get_node()->get_logger(), + "Canceling active trajectory goal because cancel callback was received." ); + } else { + RCLCPP_INFO( get_node()->get_logger(), "Received cancel request for inactive trajectory goal" ); + return rclcpp_action::CancelResponse::REJECT; + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +rclcpp_action::GoalResponse +WaypointControllerBase::goal_received_callback( const rclcpp_action::GoalUUID &, + std::shared_ptr goal ) +{ + RCLCPP_INFO( get_node()->get_logger(), "Received new trajectory goal" ); + + // Precondition: Running controller + if ( !( get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ) ) { + RCLCPP_ERROR( get_node()->get_logger(), + "Can't accept new action goals. Controller is not running." ); + return rclcpp_action::GoalResponse::REJECT; + } + + if ( !validate_trajectory( goal->waypoint_trajectory, goal->heading_trajectory ) ) { + + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +bool WaypointControllerBase::validate_trajectory( + const std::vector &point_trajectory, + const std::vector &heading_trajectory ) +{ + if ( point_trajectory.empty() || heading_trajectory.empty() ) { + RCLCPP_ERROR( get_node()->get_logger(), "Received empty trajectory, rejecting goal" ); + return false; + } + + if ( point_trajectory.size() != heading_trajectory.size() ) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with different number of waypoints and headings, rejecting goal" ); + return false; + } + + const auto current_pose = current_pose_.readFromNonRT(); + // compute distance from current position to first waypoint + double total_distance = std::sqrt( std::pow( point_trajectory[0].x - current_pose->x, 2 ) + + std::pow( point_trajectory[0].y - current_pose->y, 2 ) ) + + std::abs( heading_trajectory[0] - current_pose->heading ); + for ( size_t i = 1; i < point_trajectory.size(); ++i ) { + total_distance += std::sqrt( std::pow( point_trajectory[i].x - point_trajectory[i - 1].x, 2 ) + + std::pow( point_trajectory[i].y - point_trajectory[i - 1].y, 2 ) ) + + std::abs( heading_trajectory[i] - heading_trajectory[i - 1] ); + } + if ( total_distance <= 0.0 ) { + RCLCPP_ERROR( get_node()->get_logger(), + "Received trajectory with zero total distance, rejecting goal" ); + return false; + } + + return true; +} + +void WaypointControllerBase::preempt_active_goal( std::shared_ptr active_trajectory ) +{ + const auto res_msg = get_result_msg( false, "Goal preempted by a new goal" ); + active_trajectory->setCanceled( std::make_shared( res_msg ) ); + + trajectory_gh_buffer_.writeFromNonRT( std::shared_ptr() ); + trajectory_buffer_.writeFromNonRT( nullptr ); +} + +void WaypointControllerBase::goal_accepted_callback( + std::shared_ptr> goal_handle ) +{ + canceled_.store( true ); + + const auto active_trajectory = *trajectory_gh_buffer_.readFromNonRT(); + // Cancel any currently active goal + if ( active_trajectory ) { + preempt_active_goal( active_trajectory ); + } + + // Update the active goal handle + std::shared_ptr rt_goal = + std::make_shared( goal_handle, get_node()->get_logger() ); + rt_goal->execute(); + trajectory_gh_buffer_.writeFromNonRT( rt_goal ); + + // Update ttrajectory + trajectory_buffer_.writeFromNonRT( goal_handle->get_goal() ); + + // Delete previous timer + // Timer is kept to keep previous gh in scope until cancelation/success/aborting is processed + if ( action_monitor_timer_ ) + action_monitor_timer_->reset(); + + action_monitor_timer_ = get_node()->create_wall_timer( + action_monitor_period_, std::bind( &RtGhWayNav::runNonRealtime, rt_goal ) ); + + RCLCPP_INFO( get_node()->get_logger(), "Accepted new trajectory goal. Start executing." ); + + reset_trajectory_.store( true ); + canceled_.store( false ); +} + +bool WaypointControllerBase::stop_base() { return set_base_velocities( MoveCommand{ 0.0, 0.0 } ); } + +bool WaypointControllerBase::set_base_velocities( const MoveCommand &cmd ) +{ + bool success = true; + if ( use_cmd_vel_ ) { + auto twist_msg = geometry_msgs::msg::TwistStamped(); + twist_msg.header.stamp = this->get_node()->now(); + twist_msg.twist.linear.x = cmd.linear_vel_cmd; + twist_msg.twist.angular.z = cmd.angual_vel_cmd; + vel_pub_->publish( twist_msg ); + } else { + success = success && command_interfaces_[0].set_value( cmd.linear_vel_cmd ); // linear velocity + success = success && command_interfaces_[1].set_value( cmd.angual_vel_cmd ); // angular velocity + } + + return success; +} + +controller_interface::return_type +WaypointControllerBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) +{ + // Only overwrite trajectory & goal handle if reset flag was set by non-RT thread + if ( reset_trajectory_.exchange( false ) ) { + active_gh_ = *trajectory_gh_buffer_.readFromRT(); + active_trajectory_ = *trajectory_buffer_.readFromRT(); + + current_goal_idx_ = 0; + current_goal_ = Waypoint( active_trajectory_->waypoint_trajectory[0], + active_trajectory_->heading_trajectory[0] ); + active_ = true; + } + + if ( canceled_.load() || !active_ ) { + // Stop the robot + if ( stop_base() ) { + return controller_interface::return_type::OK; + } else { + return controller_interface::return_type::ERROR; + } + } + + update_feedback(); + + if ( check_goal_completion( current_goal_, *current_pose_.readFromRT(), + current_goal_idx_ == + ( active_trajectory_->waypoint_trajectory.size() - 1 ) ) ) { + current_goal_idx_ += 1; + + if ( current_goal_idx_ == active_trajectory_->waypoint_trajectory.size() ) { + // finished trajectory + const auto res_msg = get_result_msg( true, "" ); + active_gh_->setSucceeded( std::make_shared( res_msg ) ); + + active_ = false; + + if ( stop_base() ) { + return controller_interface::return_type::OK; + } else { + return controller_interface::return_type::ERROR; + } + } else { + // proceed to next goal + current_goal_ = Waypoint( active_trajectory_->waypoint_trajectory[current_goal_idx_], + active_trajectory_->heading_trajectory[current_goal_idx_] ); + } + } + + const auto velCmd = + computeCommand( current_goal_, *current_pose_.readFromRT(), 0.0, 0.0, + current_goal_idx_ == ( active_trajectory_->waypoint_trajectory.size() - 1 ) ); + + bool success = set_base_velocities( velCmd ); + if ( !success ) { + const auto res_msg = get_result_msg( false, "Failed to set base velocities" ); + active_gh_->setAborted( std::make_shared( res_msg ) ); + + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +void WaypointControllerBase::update_feedback() +{ + WaypointNav::Feedback feedback; + const auto pose = current_pose_.readFromRT(); + feedback.current_position.x = pose->x; + feedback.current_position.y = pose->y; + feedback.current_heading = pose->heading; + feedback.current_goal.x = current_goal_.x; + feedback.current_goal.y = current_goal_.y; + + feedback_buffer_.writeFromRT( feedback ); + active_gh_->setFeedback( std::make_shared( feedback ) ); +} + +WaypointNav::Result WaypointControllerBase::get_result_msg( bool success, + const std::string &failure_report ) +{ + WaypointNav::Result result; + const auto pose = current_pose_.readFromRT(); + result.success = success; + result.final_position.x = pose->x; + result.final_position.y = pose->y; + result.final_heading = pose->heading; + result.failure_report = failure_report; + + return result; +} +} // namespace waypoint_controller diff --git a/hector_ros_controllers_msgs/CMakeLists.txt b/hector_ros_controllers_msgs/CMakeLists.txt new file mode 100644 index 0000000..4661e2d --- /dev/null +++ b/hector_ros_controllers_msgs/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_ros_controllers_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} action/WaypointNavigation.action + DEPENDENCIES geometry_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/hector_ros_controllers_msgs/README.md b/hector_ros_controllers_msgs/README.md new file mode 100644 index 0000000..310a4a1 --- /dev/null +++ b/hector_ros_controllers_msgs/README.md @@ -0,0 +1,22 @@ +# hector_ros_controllers_msgs + +TODO + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/hector_ros_controllers_msgs/action/WaypointNavigation.action b/hector_ros_controllers_msgs/action/WaypointNavigation.action new file mode 100644 index 0000000..25e18f3 --- /dev/null +++ b/hector_ros_controllers_msgs/action/WaypointNavigation.action @@ -0,0 +1,11 @@ +geometry_msgs/Point[] waypoint_trajectory +float64[] heading_trajectory +--- +bool success +string failure_report +geometry_msgs/Point final_position +float64 final_heading +--- +geometry_msgs/Point current_goal +geometry_msgs/Point current_position +float64 current_heading diff --git a/hector_ros_controllers_msgs/package.xml b/hector_ros_controllers_msgs/package.xml new file mode 100644 index 0000000..0a10614 --- /dev/null +++ b/hector_ros_controllers_msgs/package.xml @@ -0,0 +1,24 @@ + + + + hector_ros_controllers_msgs + 0.0.0 + TODO + + md26woxu + TODO + md26woxu + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + geometry_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +