From 2bbeb05764f37733b32d5a670e60a94ac1e5362f Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 30 May 2024 19:02:34 -0700 Subject: [PATCH] Add lethal cost threshold config --- .../config/pure_pursuit_config.yaml | 1 + .../src/follower_action_server.cpp | 469 +++++++++--------- 2 files changed, 245 insertions(+), 225 deletions(-) diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml index 1c06c810..aab639fc 100644 --- a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml +++ b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml @@ -2,6 +2,7 @@ follower_action_server: ros__parameters: lookahead_distance: 0.9 desired_linear_velocity: 0.5 + lethal_cost_threshold: 10 cmd_vel_stamped: true cmd_vel_topic: "/rover_drivetrain_controller/cmd_vel" odom_topic: "/rover_drivetrain_controller/odom" diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 0d1f3433..13738de1 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -7,262 +7,281 @@ namespace follower_action_server { -FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) -: Node("follower_action_server", options) -{ - RCLCPP_INFO(this->get_logger(), "Follower node has been started."); - - declare_parameter("lookahead_distance", 1.0); - declare_parameter("desired_linear_velocity", 0.5); - declare_parameter("cmd_vel_topic", "/cmd_vel"); - declare_parameter("odom_topic", "/odom"); - declare_parameter("map_frame", "map"); - declare_parameter("goal_tolerance", 0.1); - declare_parameter("cmd_vel_stamped", false); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - stamped_ = get_parameter("cmd_vel_stamped").as_bool(); - - if (stamped_) { - cmd_vel_stamped_pub_ = - create_publisher( - get_parameter( - "cmd_vel_topic") - .as_string(), - 10); - } else { - cmd_vel_pub_ = create_publisher( - get_parameter( - "cmd_vel_topic") - .as_string(), - 10); + FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) + : Node("follower_action_server", options) + { + RCLCPP_INFO(this->get_logger(), "Follower node has been started."); + + declare_parameter("lookahead_distance", 1.0); + declare_parameter("desired_linear_velocity", 0.5); + declare_parameter("cmd_vel_topic", "/cmd_vel"); + declare_parameter("odom_topic", "/odom"); + declare_parameter("map_frame", "map"); + declare_parameter("goal_tolerance", 0.1); + declare_parameter("cmd_vel_stamped", false); + declare_parameter("lethal_cost_threshold", 50); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + stamped_ = get_parameter("cmd_vel_stamped").as_bool(); + + if (stamped_) + { + cmd_vel_stamped_pub_ = + create_publisher( + get_parameter( + "cmd_vel_topic") + .as_string(), + 10); + } + else + { + cmd_vel_pub_ = create_publisher( + get_parameter( + "cmd_vel_topic") + .as_string(), + 10); + } + + carrot_pub_ = create_publisher("carrot", 10); + marker_pub_ = create_publisher("lookahead_circle", 10); + + odom_sub_ = create_subscription( + get_parameter("odom_topic").as_string(), + 10, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) + { + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose = msg->pose.pose; + current_pose_ = pose; + }); + + // Setup the costmap + costmap_subscriber_ = create_subscription( + "/costmap", + rclcpp::SystemDefaultsQoS(), + std::bind(&FollowerActionServer::handleCostmap, this, std::placeholders::_1)); + + // Create an action server for the follow_path action + follow_path_server_ = rclcpp_action::create_server( + this, + "follow_path", + std::bind( + &FollowerActionServer::handle_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&FollowerActionServer::handle_cancel, this, std::placeholders::_1), + std::bind(&FollowerActionServer::handle_accepted, this, std::placeholders::_1)); } - carrot_pub_ = create_publisher("carrot", 10); - marker_pub_ = create_publisher("lookahead_circle", 10); + void FollowerActionServer::handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) + { + current_costmap_ = *msg; + } - odom_sub_ = create_subscription( - get_parameter("odom_topic").as_string(), - 10, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) + geometry_msgs::msg::TransformStamped FollowerActionServer::lookup_transform( + std::string target_frame, std::string source_frame) + { + geometry_msgs::msg::TransformStamped transform; + try { - geometry_msgs::msg::PoseStamped pose; - pose.header = msg->header; - pose.pose = msg->pose.pose; - current_pose_ = pose; - }); - - // Setup the costmap - costmap_subscriber_ = create_subscription( - "/costmap", - rclcpp::SystemDefaultsQoS(), - std::bind(&FollowerActionServer::handleCostmap, this, std::placeholders::_1)); - - // Create an action server for the follow_path action - follow_path_server_ = rclcpp_action::create_server( - this, - "follow_path", - std::bind( - &FollowerActionServer::handle_goal, this, std::placeholders::_1, - std::placeholders::_2), - std::bind(&FollowerActionServer::handle_cancel, this, std::placeholders::_1), - std::bind(&FollowerActionServer::handle_accepted, this, std::placeholders::_1)); -} - -void FollowerActionServer::handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -{ - current_costmap_ = *msg; -} + transform = tf_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } + catch (tf2::TransformException &ex) + { + RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what()); + } + return transform; + } -geometry_msgs::msg::TransformStamped FollowerActionServer::lookup_transform( - std::string target_frame, std::string source_frame) -{ - geometry_msgs::msg::TransformStamped transform; - try { - transform = tf_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what()); + FollowerActionServer::~FollowerActionServer() + { + RCLCPP_INFO(this->get_logger(), "Follower action server has been stopped."); } - return transform; -} -FollowerActionServer::~FollowerActionServer() -{ - RCLCPP_INFO(this->get_logger(), "Follower action server has been stopped."); -} + rclcpp_action::GoalResponse FollowerActionServer::handle_goal( + const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) + { + RCLCPP_INFO(this->get_logger(), "Received goal request"); -rclcpp_action::GoalResponse FollowerActionServer::handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) -{ - RCLCPP_INFO(this->get_logger(), "Received goal request"); + (void)uuid; + (void)goal; - (void)uuid; - (void)goal; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} + rclcpp_action::CancelResponse FollowerActionServer::handle_cancel( + const std::shared_ptr> goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); -rclcpp_action::CancelResponse FollowerActionServer::handle_cancel( - const std::shared_ptr> goal_handle) -{ - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; - (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - return rclcpp_action::CancelResponse::ACCEPT; -} + void FollowerActionServer::handle_accepted( + const std::shared_ptr> goal_handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FollowerActionServer::execute, this, std::placeholders::_1), + goal_handle} + .detach(); + } -void FollowerActionServer::handle_accepted( - const std::shared_ptr> goal_handle) -{ - // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&FollowerActionServer::execute, this, std::placeholders::_1), - goal_handle} - .detach(); -} - -visualization_msgs::msg::Marker FollowerActionServer::create_lookahead_circle( - double x, double y, - double radius, - std::string frame_id) -{ - visualization_msgs::msg::Marker circle; - circle.header.frame_id = frame_id; - circle.header.stamp = get_clock()->now(); - uint32_t shape = visualization_msgs::msg::Marker::CYLINDER; - - circle.ns = "basic_shapes"; - circle.id = 0; - circle.type = shape; - circle.action = visualization_msgs::msg::Marker::ADD; - - circle.pose.position.x = x; - circle.pose.position.y = y; - circle.pose.position.z = 0.0; - circle.pose.orientation.x = 0.0; - circle.pose.orientation.y = 0.0; - circle.pose.orientation.z = 0.0; - circle.pose.orientation.w = 1.0; - - circle.scale.x = 2 * radius; - circle.scale.y = 2 * radius; - circle.scale.z = 0.01; - - circle.color.r = 0.0f; - circle.color.g = 0.0f; - circle.color.b = 1.0f; - circle.color.a = 0.3; - - return circle; -} - -void FollowerActionServer::publishZeroVelocity() -{ - geometry_msgs::msg::TwistStamped cmd_vel; - cmd_vel.header.stamp = get_clock()->now(); - cmd_vel.twist.linear.x = 0.0; - cmd_vel.twist.angular.z = 0.0; - - if (stamped_) { - cmd_vel_stamped_pub_->publish(cmd_vel); - } else { - cmd_vel_pub_->publish(cmd_vel.twist); + visualization_msgs::msg::Marker FollowerActionServer::create_lookahead_circle( + double x, double y, + double radius, + std::string frame_id) + { + visualization_msgs::msg::Marker circle; + circle.header.frame_id = frame_id; + circle.header.stamp = get_clock()->now(); + uint32_t shape = visualization_msgs::msg::Marker::CYLINDER; + + circle.ns = "basic_shapes"; + circle.id = 0; + circle.type = shape; + circle.action = visualization_msgs::msg::Marker::ADD; + + circle.pose.position.x = x; + circle.pose.position.y = y; + circle.pose.position.z = 0.0; + circle.pose.orientation.x = 0.0; + circle.pose.orientation.y = 0.0; + circle.pose.orientation.z = 0.0; + circle.pose.orientation.w = 1.0; + + circle.scale.x = 2 * radius; + circle.scale.y = 2 * radius; + circle.scale.z = 0.01; + + circle.color.r = 0.0f; + circle.color.g = 0.0f; + circle.color.b = 1.0f; + circle.color.a = 0.3; + + return circle; } -} -int FollowerActionServer::getCost(const nav_msgs::msg::OccupancyGrid & costmap, double x, double y) -{ - int map_x = (x - costmap.info.origin.position.x) / costmap.info.resolution; - int map_y = (y - costmap.info.origin.position.y) / costmap.info.resolution; + void FollowerActionServer::publishZeroVelocity() + { + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header.stamp = get_clock()->now(); + cmd_vel.twist.linear.x = 0.0; + cmd_vel.twist.angular.z = 0.0; - if (map_x < 0 || map_x >= costmap.info.width || map_y < 0 || map_y >= costmap.info.height) { - return 0; + if (stamped_) + { + cmd_vel_stamped_pub_->publish(cmd_vel); + } + else + { + cmd_vel_pub_->publish(cmd_vel.twist); + } } - return costmap.data[map_y * costmap.info.width + map_x]; -} + int FollowerActionServer::getCost(const nav_msgs::msg::OccupancyGrid &costmap, double x, double y) + { + int map_x = (x - costmap.info.origin.position.x) / costmap.info.resolution; + int map_y = (y - costmap.info.origin.position.y) / costmap.info.resolution; -void FollowerActionServer::execute( - const std::shared_ptr> goal_handle) -{ - RCLCPP_INFO(this->get_logger(), "Executing goal"); - - auto feedback = std::make_shared(); - feedback->distance_to_goal = std::numeric_limits::max(); - - auto result = std::make_shared(); - auto & path = goal_handle->get_goal()->path; - - // Create a PurePursuit object - pure_pursuit::PurePursuitParams params; - params.lookahead_distance = get_parameter("lookahead_distance").as_double(); - params.desired_linear_velocity = get_parameter("desired_linear_velocity").as_double(); - pure_pursuit::PurePursuit pure_pursuit(params); - - pure_pursuit.setPath(path); - - pure_pursuit::PurePursuitOutput output; - rclcpp::Rate rate(10); - - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Goal has been canceled"); - break; - } else if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double()) { - result->error_code = urc_msgs::action::FollowPath::Result::SUCCESS; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal has been reached!"); - break; - } else if (getCost( - current_costmap_, output.lookahead_point.point.x, - output.lookahead_point.point.y) > 0) + if (map_x < 0 || map_x >= costmap.info.width || map_y < 0 || map_y >= costmap.info.height) { - result->error_code = urc_msgs::action::FollowPath::Result::OBSTACLE_DETECTED; - goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Obstacle detected!"); - break; + return 0; } - output = - pure_pursuit.getCommandVelocity( - lookup_transform( - "base_link", - get_parameter("map_frame").as_string())); + return costmap.data[map_y * costmap.info.width + map_x]; + } - auto odom_to_map_ = lookup_transform(get_parameter("map_frame").as_string(), "odom"); + void FollowerActionServer::execute( + const std::shared_ptr> goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); - if (stamped_) { - cmd_vel_stamped_pub_->publish(output.cmd_vel); - } else { - cmd_vel_pub_->publish(output.cmd_vel.twist); - } + auto feedback = std::make_shared(); + feedback->distance_to_goal = std::numeric_limits::max(); - geometry_msgs::msg::PoseStamped current_pose_map_frame_; - tf2::doTransform(current_pose_, current_pose_map_frame_, odom_to_map_); + auto result = std::make_shared(); + auto &path = goal_handle->get_goal()->path; - auto circle = - create_lookahead_circle( - current_pose_map_frame_.pose.position.x, current_pose_map_frame_.pose.position.y, - params.lookahead_distance, get_parameter("map_frame").as_string()); - marker_pub_->publish(circle); + // Create a PurePursuit object + pure_pursuit::PurePursuitParams params; + params.lookahead_distance = get_parameter("lookahead_distance").as_double(); + params.desired_linear_velocity = get_parameter("desired_linear_velocity").as_double(); + pure_pursuit::PurePursuit pure_pursuit(params); - // Publish the carrot point - carrot_pub_->publish(output.lookahead_point); + pure_pursuit.setPath(path); - // Publish feedback - feedback->distance_to_goal = - geometry_util::dist2D(current_pose_map_frame_.pose.position, path.poses.back().pose.position); - goal_handle->publish_feedback(feedback); + pure_pursuit::PurePursuitOutput output; + rclcpp::Rate rate(10); - rate.sleep(); - } + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal has been canceled"); + break; + } + else if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double()) + { + result->error_code = urc_msgs::action::FollowPath::Result::SUCCESS; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal has been reached!"); + break; + } + else if (getCost( + current_costmap_, output.lookahead_point.point.x, + output.lookahead_point.point.y) > get_parameter("lethal_cost_threshold").as_int()) + { + result->error_code = urc_msgs::action::FollowPath::Result::OBSTACLE_DETECTED; + goal_handle->abort(result); + RCLCPP_INFO(this->get_logger(), "Obstacle detected!"); + break; + } + + output = + pure_pursuit.getCommandVelocity( + lookup_transform( + "base_link", + get_parameter("map_frame").as_string())); + + auto odom_to_map_ = lookup_transform(get_parameter("map_frame").as_string(), "odom"); + + if (stamped_) + { + cmd_vel_stamped_pub_->publish(output.cmd_vel); + } + else + { + cmd_vel_pub_->publish(output.cmd_vel.twist); + } + + geometry_msgs::msg::PoseStamped current_pose_map_frame_; + tf2::doTransform(current_pose_, current_pose_map_frame_, odom_to_map_); + + auto circle = + create_lookahead_circle( + current_pose_map_frame_.pose.position.x, current_pose_map_frame_.pose.position.y, + params.lookahead_distance, get_parameter("map_frame").as_string()); + marker_pub_->publish(circle); + + // Publish the carrot point + carrot_pub_->publish(output.lookahead_point); + + // Publish feedback + feedback->distance_to_goal = + geometry_util::dist2D(current_pose_map_frame_.pose.position, path.poses.back().pose.position); + goal_handle->publish_feedback(feedback); + + rate.sleep(); + } - publishZeroVelocity(); -} + publishZeroVelocity(); + } } // namespace follower_node