16 #ifndef OPENNAV_DOCKING__CONTROLLER_HPP_
17 #define OPENNAV_DOCKING__CONTROLLER_HPP_
23 #include "geometry_msgs/msg/pose.hpp"
24 #include "geometry_msgs/msg/transform_stamped.hpp"
25 #include "geometry_msgs/msg/twist.hpp"
26 #include "nav2_costmap_2d/costmap_subscriber.hpp"
27 #include "nav2_costmap_2d/footprint_subscriber.hpp"
28 #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
29 #include "nav2_graceful_controller/smooth_control_law.hpp"
30 #include "nav_msgs/msg/path.hpp"
31 #include "nav2_ros_common/lifecycle_node.hpp"
32 #include "nav2_ros_common/tf2_factories.hpp"
34 namespace opennav_docking
51 const nav2::LifecycleNode::SharedPtr & node, nav2::TransformBuffer::SharedPtr tf,
52 std::string base_frame);
69 const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd,
70 const geometry_msgs::msg::TransformStamped & base_to_fixed_transform,
71 bool is_docking,
bool backward =
false);
81 const double & angular_distance_to_heading,
82 const geometry_msgs::msg::Twist & current_velocity,
96 const geometry_msgs::msg::Pose & target_pose,
97 const geometry_msgs::msg::TransformStamped & base_to_fixed_transform,
98 bool is_docking,
bool backward =
false);
109 const std::vector<rclcpp::Parameter> & parameters);
128 const nav2::LifecycleNode::SharedPtr & node,
129 std::string costmap_topic, std::string footprint_topic,
double transform_tolerance);
132 rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
133 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
134 std::mutex dynamic_params_lock_;
136 rclcpp::Logger logger_{rclcpp::get_logger(
"Controller")};
138 std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
139 double k_phi_, k_delta_, beta_, lambda_;
140 double slowdown_radius_, deceleration_max_, v_linear_min_, v_linear_max_, v_angular_max_;
141 double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_;
144 nav2::Publisher<nav_msgs::msg::Path>::SharedPtr trajectory_pub_;
147 bool use_collision_detection_;
148 double projection_time_;
149 double simulation_time_step_;
150 double dock_collision_threshold_;
151 double transform_tolerance_;
152 nav2::TransformBuffer::SharedPtr tf2_buffer_;
153 std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
154 std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
155 std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
156 std::string base_frame_;
Default control law for approaching a dock target.
void updateParametersCallback(const std::vector< rclcpp::Parameter > ¶meters)
Apply parameter updates after validation This callback is executed when parameters have been successf...
bool computeVelocityCommand(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Twist &cmd, const geometry_msgs::msg::TransformStamped &base_to_fixed_transform, bool is_docking, bool backward=false)
Compute a velocity command using control law.
~Controller()
A destructor for opennav_docking::Controller.
geometry_msgs::msg::Twist computeRotateToHeadingCommand(const double &angular_distance_to_heading, const geometry_msgs::msg::Twist ¤t_velocity, const double &dt)
Perform a command for in-place rotation.
bool isTrajectoryCollisionFree(const geometry_msgs::msg::Pose &target_pose, const geometry_msgs::msg::TransformStamped &base_to_fixed_transform, bool is_docking, bool backward=false)
Check if a trajectory is collision free.
Controller(const nav2::LifecycleNode::SharedPtr &node, nav2::TransformBuffer::SharedPtr tf, std::string base_frame)
Create a controller instance. Configure ROS 2 parameters.
void configureCollisionChecker(const nav2::LifecycleNode::SharedPtr &node, std::string costmap_topic, std::string footprint_topic, double transform_tolerance)
Configure the collision checker.
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > ¶meters)
Validate incoming parameter updates before applying them. This callback is triggered when one or more...