diff --git a/include/social_force_window_planner/sfw_planner_node.hpp b/include/social_force_window_planner/sfw_planner_node.hpp index bb45bed..8a2b502 100644 --- a/include/social_force_window_planner/sfw_planner_node.hpp +++ b/include/social_force_window_planner/sfw_planner_node.hpp @@ -20,6 +20,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/controller.hpp" +#include "nav2_core/goal_checker.hpp" #include "nav2_core/exceptions.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" @@ -71,9 +72,9 @@ class SFWPlannerNode : public nav2_core::Controller { * @param costmap_ros Costmap2DROS object of environment */ void - configure(const rclcpp_lifecycle::LifecycleNode::SharedPtr &parent, - const std::string name, const std::shared_ptr &tf, - const std::shared_ptr &costmap_ros) + configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, + const std::string name, const std::shared_ptr tf, + const std::shared_ptr costmap_ros) override; /** @@ -107,7 +108,8 @@ class SFWPlannerNode : public nav2_core::Controller { */ geometry_msgs::msg::TwistStamped computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, - const geometry_msgs::msg::Twist &velocity) override; + const geometry_msgs::msg::Twist &velocity, + nav2_core::GoalChecker * goal_checker) override; /** * @brief nav2_core setPlan - Sets the global plan @@ -115,6 +117,13 @@ class SFWPlannerNode : public nav2_core::Controller { */ void setPlan(const nav_msgs::msg::Path &path) override; + /** + * @brief Set new speed limit from callback + * @param speed_limit Speed limit to use + * @param percentage Bool if the speed limit is absolute or relative + */ + void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + protected: /** * @brief Transforms global plan into same frame as pose (robot_pose), clips @@ -151,7 +160,7 @@ class SFWPlannerNode : public nav2_core::Controller { bool isGoalReached(); - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::string name_; std::shared_ptr tf_; std::string plugin_name_; diff --git a/src/sensor_interface.cpp b/src/sensor_interface.cpp index 9de0046..21b5388 100644 --- a/src/sensor_interface.cpp +++ b/src/sensor_interface.cpp @@ -319,7 +319,7 @@ void SFMSensorInterface::publish_obstacle_points( m.color.b = 0.0; m.color.a = 1.0; m.id = 1000; - m.lifetime = rclcpp::Duration(0.3); + m.lifetime = rclcpp::Duration::from_seconds(0.3); // printf("Published Obstacles: "); for (utils::Vector2d p : points) { geometry_msgs::msg::Point pt; diff --git a/src/sfw_planner.cpp b/src/sfw_planner.cpp index 9dc9d99..21c33f0 100644 --- a/src/sfw_planner.cpp +++ b/src/sfw_planner.cpp @@ -100,7 +100,7 @@ void SFWPlanner::initializeMarkers() { markers_.markers[counter].id = counter; markers_.markers[counter].type = 4; markers_.markers[counter].action = 0; // 0 - add/modify an object - markers_.markers[counter].lifetime = rclcpp::Duration(0.3); + markers_.markers[counter].lifetime = rclcpp::Duration::from_seconds(0.3); markers_.markers[counter].scale.x = 0.01; markers_.markers[counter].color.a = 1.0; markers_.markers[counter].pose.orientation.w = 1.0; diff --git a/src/sfw_planner_node.cpp b/src/sfw_planner_node.cpp index 7cdfd26..512632b 100644 --- a/src/sfw_planner_node.cpp +++ b/src/sfw_planner_node.cpp @@ -45,12 +45,13 @@ Iter min_by(Iter begin, Iter end, Getter getCompareVal) { } void SFWPlannerNode::configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr &parent, - const std::string name, const std::shared_ptr &tf, - const std::shared_ptr &costmap_ros) { + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, + const std::string name, const std::shared_ptr tf, + const std::shared_ptr costmap_ros) { - logger_ = parent->get_logger(); node_ = parent; + auto node = node_.lock(); + name_ = name; RCLCPP_INFO(logger_, @@ -63,19 +64,19 @@ void SFWPlannerNode::configure( tf_ = tf; // sensor interface - sensor_iface_ = std::make_shared(parent, tf, name); + sensor_iface_ = std::make_shared(node, tf, name); global_path_pub_ = - parent->create_publisher("robot_global_plan", 1); + node->create_publisher("robot_global_plan", 1); local_path_pub_ = - parent->create_publisher("robot_local_plan", 1); + node->create_publisher("robot_local_plan", 1); - traj_pub_ = parent->create_publisher( + traj_pub_ = node->create_publisher( "robot_local_trajectories", 1); sfw_planner_ = - std::make_shared(parent, name, sensor_iface_, *costmap_, + std::make_shared(node, name, sensor_iface_, *costmap_, costmap_ros_->getRobotFootprint()); } @@ -219,8 +220,10 @@ bool SFWPlannerNode::transformPoint( geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands( const geometry_msgs::msg::PoseStamped &pose, - const geometry_msgs::msg::Twist &speed) { + const geometry_msgs::msg::Twist &speed, + nav2_core::GoalChecker * goal_checker) { + auto node = node_.lock(); // RCLCPP_INFO(logger_, "ComputeVelocityCommands called!!!!"); geometry_msgs::msg::TwistStamped velStamp; sensor_iface_->start(); @@ -237,6 +240,13 @@ geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands( "Unable to transform robot pose into costmap's frame"); } + // Update for the current goal checker's state + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist vel_tolerance; + if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) { + RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!"); + } + // TODO: Check here if we have already reached the goal // std::vector transformed_plan; @@ -293,7 +303,7 @@ geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands( // pass along drive commands if (!ok) { - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG(node->get_logger(), "The sfw planner failed to find a valid plan. This " "means that the footprint of the robot was in collision " "for all simulated trajectories."); @@ -308,7 +318,7 @@ geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands( global_path_pub_->publish(transformed_plan); traj_pub_->publish(markers); - velStamp.header.stamp = node_->get_clock()->now(); + velStamp.header.stamp = node->get_clock()->now(); velStamp.header.frame_id = costmap_ros_->getGlobalFrameID(); velStamp.twist = drive_cmds; return velStamp;