From 1041de9ee10cbd08bde67ed5fe16a0cf3dc43ef2 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 30 May 2024 11:09:31 -0700 Subject: [PATCH 01/11] Initial commit, not working --- urc_behaviors/CMakeLists.txt | 82 +++++++++++++ urc_behaviors/include/search_for_aruco.hpp | 46 ++++++++ urc_behaviors/package.xml | 29 +++++ urc_behaviors/src/search_for_aruco.cpp | 110 ++++++++++++++++++ .../launch/bringup_simulation.launch.py | 6 +- urc_bringup/strategies/bt_test.xml | 7 -- .../include/follower_action_server.hpp | 82 ++++++------- 7 files changed, 310 insertions(+), 52 deletions(-) create mode 100644 urc_behaviors/CMakeLists.txt create mode 100644 urc_behaviors/include/search_for_aruco.hpp create mode 100644 urc_behaviors/package.xml create mode 100644 urc_behaviors/src/search_for_aruco.cpp diff --git a/urc_behaviors/CMakeLists.txt b/urc_behaviors/CMakeLists.txt new file mode 100644 index 00000000..2a0421f5 --- /dev/null +++ b/urc_behaviors/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.5) +project(urc_behaviors) + +include(../cmake/default_settings.cmake) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(urc_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories( + include +) + +add_library(${PROJECT_NAME} SHARED + src/search_for_aruco.cpp +) + +set(dependencies + rclcpp + rclcpp_components + urc_msgs + std_msgs + sensor_msgs + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + nav_msgs +) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) + +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "urc_behaviors::SearchForAruco" + EXECUTABLE ${PROJECT_NAME}_SearchForAruco +) + +# Install launch files. +install( + DIRECTORY + DESTINATION share/${PROJECT_NAME}/ +) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(msg) + +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/urc_behaviors/include/search_for_aruco.hpp b/urc_behaviors/include/search_for_aruco.hpp new file mode 100644 index 00000000..c1ab868f --- /dev/null +++ b/urc_behaviors/include/search_for_aruco.hpp @@ -0,0 +1,46 @@ +#ifndef SEARCH_FOR_ARUCO_HPP_ +#define SEARCH_FOR_ARUCO_HPP_ + +#include + +#include +#include +#include + +#include + +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +namespace urc_behaviors +{ + class SearchForAruco : public rclcpp::Node + { + public: + using FollowPath = urc_msgs::action::FollowPath; + using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle; + + explicit SearchForAruco(const rclcpp::NodeOptions &options); + + ~SearchForAruco() = default; + + private: + void search(); + + nav_msgs::msg::Path generate_search_path(double spiral_coeff); + + void goal_response_callback(const GoalHandleFollowPath::SharedPtr &goal_handle); + void result_callback(const GoalHandleFollowPath::WrappedResult &result); + void feedback_callback(GoalHandleFollowPath::SharedPtr, const std::shared_ptr feedback); + + rclcpp_action::Client::SharedPtr follow_path_client_; + + bool aruco_seen_; + rclcpp::Time aruco_first_seen_time_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + }; +} // namespace urc_behaviors + +#endif // SEARCH_FOR_ARUCO_HPP_ diff --git a/urc_behaviors/package.xml b/urc_behaviors/package.xml new file mode 100644 index 00000000..9a5c9fcd --- /dev/null +++ b/urc_behaviors/package.xml @@ -0,0 +1,29 @@ + + + + urc_behaviors + 0.0.0 + TODO: Package description + yashas + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_components + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + urc_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp new file mode 100644 index 00000000..75df9da3 --- /dev/null +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -0,0 +1,110 @@ +#include "search_for_aruco.hpp" +#include + +namespace urc_behaviors +{ + SearchForAruco::SearchForAruco(const rclcpp::NodeOptions &options) : Node("search_for_aruco", options) + { + RCLCPP_INFO(this->get_logger(), "Search for ARUCO behavior server has been started."); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + follow_path_client_ = rclcpp_action::create_client(this, "follow_path"); + + search(); + } + + void SearchForAruco::search() + { + // Generate a search path + auto path = generate_search_path(0.1); + + // Send the path to the follow_path action server + auto goal = FollowPath::Goal(); + goal.path = path; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = std::bind(&SearchForAruco::goal_response_callback, this, std::placeholders::_1); + send_goal_options.result_callback = std::bind(&SearchForAruco::result_callback, this, std::placeholders::_1); + send_goal_options.feedback_callback = std::bind(&SearchForAruco::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + + follow_path_client_->async_send_goal(goal, send_goal_options); + } + + void SearchForAruco::goal_response_callback(const GoalHandleFollowPath::SharedPtr &goal_handle) + { + if (!goal_handle) + { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } + else + { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + } + + void SearchForAruco::result_callback(const GoalHandleFollowPath::WrappedResult &result) + { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_INFO(this->get_logger(), "Goal was aborted"); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_INFO(this->get_logger(), "Goal was canceled"); + break; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + break; + } + } + + void SearchForAruco::feedback_callback(GoalHandleFollowPath::SharedPtr, const std::shared_ptr feedback) + { + RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance_to_goal); + } + + nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) + { + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = this->now(); + + // Lookup transform from base_link to map + geometry_msgs::msg::TransformStamped transform; + try + { + transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + } + catch (tf2::TransformException &ex) + { + RCLCPP_ERROR(this->get_logger(), "Failed to lookup transform: %s", ex.what()); + throw std::runtime_error("Failed to lookup transform."); + } + + for (double t = 0; t < 12 * M_PI; t += M_PI / 4) + { + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + + pose.pose.position.x = spiral_coeff * t * cos(t); + pose.pose.position.y = spiral_coeff * t * sin(t); + pose.pose.orientation.w = 1.0; + + // Transform pose to map frame + geometry_msgs::msg::PoseStamped transformed_pose; + tf2::doTransform(pose, transformed_pose, transform); + + path.poses.push_back(transformed_pose); + } + + return path; + } +} // namespace urc_behaviors + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::SearchForAruco) \ No newline at end of file diff --git a/urc_bringup/launch/bringup_simulation.launch.py b/urc_bringup/launch/bringup_simulation.launch.py index 04d3753d..bd55d04c 100644 --- a/urc_bringup/launch/bringup_simulation.launch.py +++ b/urc_bringup/launch/bringup_simulation.launch.py @@ -29,8 +29,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration("use_sim_time", default="true") xacro_file = os.path.join( - get_package_share_directory("urc_hw_description"), - "urdf/walli_sim.xacro" + get_package_share_directory("urc_hw_description"), "urdf/walli.xacro" ) assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file) robot_description_config = process_file( @@ -186,7 +185,6 @@ def generate_launch_description(): on_exit=[ load_joint_state_broadcaster, load_drivetrain_controller, - teleop_launch, map_to_odom_launch, ], ) @@ -203,7 +201,7 @@ def generate_launch_description(): on_start=[ path_planning_launch, trajectory_following_launch, - bt_launch, + # bt_launch, ], ) ), diff --git a/urc_bringup/strategies/bt_test.xml b/urc_bringup/strategies/bt_test.xml index 845c9bb1..a824057a 100644 --- a/urc_bringup/strategies/bt_test.xml +++ b/urc_bringup/strategies/bt_test.xml @@ -7,8 +7,6 @@ - - @@ -52,11 +50,6 @@ Message to log. - - - - diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 98382c10..7db93a33 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -16,61 +16,61 @@ namespace follower_action_server { -class FollowerActionServer : public rclcpp::Node -{ -public: - explicit FollowerActionServer(const rclcpp::NodeOptions & options); + class FollowerActionServer : public rclcpp::Node + { + public: + explicit FollowerActionServer(const rclcpp::NodeOptions &options); - ~FollowerActionServer(); + ~FollowerActionServer(); -private: - geometry_msgs::msg::TransformStamped lookup_transform( - std::string target_frame, - std::string source_frame); + private: + geometry_msgs::msg::TransformStamped lookup_transform( + std::string target_frame, + std::string source_frame); - visualization_msgs::msg::Marker create_lookahead_circle( - double x, double y, double radius, - std::string frame_id); + visualization_msgs::msg::Marker create_lookahead_circle( + double x, double y, double radius, + std::string frame_id); - void publishZeroVelocity(); + void publishZeroVelocity(); - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); - void handle_accepted( - const std::shared_ptr> goal_handle); + void handle_accepted( + const std::shared_ptr> goal_handle); - void execute( - const std::shared_ptr> goal_handle); + void execute( + const std::shared_ptr> goal_handle); - /** - * @brief Handle the costmap data - * @param msg The costmap data - */ - void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /** + * @brief Handle the costmap data + * @param msg The costmap data + */ + void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); - int getCost(const nav_msgs::msg::OccupancyGrid & costmap, double x, double y); + int getCost(const nav_msgs::msg::OccupancyGrid &costmap, double x, double y); - nav_msgs::msg::OccupancyGrid current_costmap_; - rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Publisher::SharedPtr carrot_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; - rclcpp_action::Server::SharedPtr follow_path_server_; - rclcpp::Subscription::SharedPtr odom_sub_; + nav_msgs::msg::OccupancyGrid current_costmap_; + rclcpp::Subscription::SharedPtr costmap_subscriber_; + rclcpp::Publisher::SharedPtr carrot_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; + rclcpp_action::Server::SharedPtr follow_path_server_; + rclcpp::Subscription::SharedPtr odom_sub_; - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; - rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; - geometry_msgs::msg::PoseStamped current_pose_; - bool stamped_; -}; + geometry_msgs::msg::PoseStamped current_pose_; + bool stamped_; + }; } // namespace follower_action_server #endif // FOLLOWER_ACTION_SERVER_HPP_ From d80c0302417a4092c2023b531e36970e8785c8e6 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 30 May 2024 11:11:46 -0700 Subject: [PATCH 02/11] Run formatter --- urc_behaviors/include/search_for_aruco.hpp | 40 ++-- urc_behaviors/src/search_for_aruco.cpp | 207 +++++++++--------- .../include/follower_action_server.hpp | 82 +++---- 3 files changed, 167 insertions(+), 162 deletions(-) diff --git a/urc_behaviors/include/search_for_aruco.hpp b/urc_behaviors/include/search_for_aruco.hpp index c1ab868f..1141af66 100644 --- a/urc_behaviors/include/search_for_aruco.hpp +++ b/urc_behaviors/include/search_for_aruco.hpp @@ -14,33 +14,35 @@ namespace urc_behaviors { - class SearchForAruco : public rclcpp::Node - { - public: - using FollowPath = urc_msgs::action::FollowPath; - using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle; +class SearchForAruco : public rclcpp::Node +{ +public: + using FollowPath = urc_msgs::action::FollowPath; + using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle; - explicit SearchForAruco(const rclcpp::NodeOptions &options); + explicit SearchForAruco(const rclcpp::NodeOptions & options); - ~SearchForAruco() = default; + ~SearchForAruco() = default; - private: - void search(); +private: + void search(); - nav_msgs::msg::Path generate_search_path(double spiral_coeff); + nav_msgs::msg::Path generate_search_path(double spiral_coeff); - void goal_response_callback(const GoalHandleFollowPath::SharedPtr &goal_handle); - void result_callback(const GoalHandleFollowPath::WrappedResult &result); - void feedback_callback(GoalHandleFollowPath::SharedPtr, const std::shared_ptr feedback); + void goal_response_callback(const GoalHandleFollowPath::SharedPtr & goal_handle); + void result_callback(const GoalHandleFollowPath::WrappedResult & result); + void feedback_callback( + GoalHandleFollowPath::SharedPtr, + const std::shared_ptr feedback); - rclcpp_action::Client::SharedPtr follow_path_client_; + rclcpp_action::Client::SharedPtr follow_path_client_; - bool aruco_seen_; - rclcpp::Time aruco_first_seen_time_; + bool aruco_seen_; + rclcpp::Time aruco_first_seen_time_; - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; - }; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; } // namespace urc_behaviors #endif // SEARCH_FOR_ARUCO_HPP_ diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index 75df9da3..75164a40 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -3,108 +3,111 @@ namespace urc_behaviors { - SearchForAruco::SearchForAruco(const rclcpp::NodeOptions &options) : Node("search_for_aruco", options) - { - RCLCPP_INFO(this->get_logger(), "Search for ARUCO behavior server has been started."); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - follow_path_client_ = rclcpp_action::create_client(this, "follow_path"); - - search(); - } - - void SearchForAruco::search() - { - // Generate a search path - auto path = generate_search_path(0.1); - - // Send the path to the follow_path action server - auto goal = FollowPath::Goal(); - goal.path = path; - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = std::bind(&SearchForAruco::goal_response_callback, this, std::placeholders::_1); - send_goal_options.result_callback = std::bind(&SearchForAruco::result_callback, this, std::placeholders::_1); - send_goal_options.feedback_callback = std::bind(&SearchForAruco::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); - - follow_path_client_->async_send_goal(goal, send_goal_options); - } - - void SearchForAruco::goal_response_callback(const GoalHandleFollowPath::SharedPtr &goal_handle) - { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - } - - void SearchForAruco::result_callback(const GoalHandleFollowPath::WrappedResult &result) - { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(this->get_logger(), "Goal succeeded"); - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_INFO(this->get_logger(), "Goal was aborted"); - break; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_INFO(this->get_logger(), "Goal was canceled"); - break; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - break; - } - } - - void SearchForAruco::feedback_callback(GoalHandleFollowPath::SharedPtr, const std::shared_ptr feedback) - { - RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance_to_goal); - } - - nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) - { - nav_msgs::msg::Path path; - path.header.frame_id = "map"; - path.header.stamp = this->now(); - - // Lookup transform from base_link to map - geometry_msgs::msg::TransformStamped transform; - try - { - transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); - } - catch (tf2::TransformException &ex) - { - RCLCPP_ERROR(this->get_logger(), "Failed to lookup transform: %s", ex.what()); - throw std::runtime_error("Failed to lookup transform."); - } - - for (double t = 0; t < 12 * M_PI; t += M_PI / 4) - { - geometry_msgs::msg::PoseStamped pose; - pose.header = path.header; - - pose.pose.position.x = spiral_coeff * t * cos(t); - pose.pose.position.y = spiral_coeff * t * sin(t); - pose.pose.orientation.w = 1.0; - - // Transform pose to map frame - geometry_msgs::msg::PoseStamped transformed_pose; - tf2::doTransform(pose, transformed_pose, transform); - - path.poses.push_back(transformed_pose); - } - - return path; - } +SearchForAruco::SearchForAruco(const rclcpp::NodeOptions & options) +: Node("search_for_aruco", options) +{ + RCLCPP_INFO(this->get_logger(), "Search for ARUCO behavior server has been started."); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + follow_path_client_ = rclcpp_action::create_client( + this, + "follow_path"); + + search(); +} + +void SearchForAruco::search() +{ + // Generate a search path + auto path = generate_search_path(0.1); + + // Send the path to the follow_path action server + auto goal = FollowPath::Goal(); + goal.path = path; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = std::bind( + &SearchForAruco::goal_response_callback, + this, std::placeholders::_1); + send_goal_options.result_callback = std::bind( + &SearchForAruco::result_callback, this, + std::placeholders::_1); + send_goal_options.feedback_callback = std::bind( + &SearchForAruco::feedback_callback, this, + std::placeholders::_1, std::placeholders::_2); + + follow_path_client_->async_send_goal(goal, send_goal_options); +} + +void SearchForAruco::goal_response_callback(const GoalHandleFollowPath::SharedPtr & goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } +} + +void SearchForAruco::result_callback(const GoalHandleFollowPath::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_INFO(this->get_logger(), "Goal was aborted"); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_INFO(this->get_logger(), "Goal was canceled"); + break; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + break; + } +} + +void SearchForAruco::feedback_callback( + GoalHandleFollowPath::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance_to_goal); +} + +nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) +{ + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = this->now(); + + // Lookup transform from base_link to map + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "Failed to lookup transform: %s", ex.what()); + throw std::runtime_error("Failed to lookup transform."); + } + + for (double t = 0; t < 12 * M_PI; t += M_PI / 4) { + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + + pose.pose.position.x = spiral_coeff * t * cos(t); + pose.pose.position.y = spiral_coeff * t * sin(t); + pose.pose.orientation.w = 1.0; + + // Transform pose to map frame + geometry_msgs::msg::PoseStamped transformed_pose; + tf2::doTransform(pose, transformed_pose, transform); + + path.poses.push_back(transformed_pose); + } + + return path; +} } // namespace urc_behaviors #include -RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::SearchForAruco) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::SearchForAruco) diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 7db93a33..98382c10 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -16,61 +16,61 @@ namespace follower_action_server { - class FollowerActionServer : public rclcpp::Node - { - public: - explicit FollowerActionServer(const rclcpp::NodeOptions &options); +class FollowerActionServer : public rclcpp::Node +{ +public: + explicit FollowerActionServer(const rclcpp::NodeOptions & options); - ~FollowerActionServer(); + ~FollowerActionServer(); - private: - geometry_msgs::msg::TransformStamped lookup_transform( - std::string target_frame, - std::string source_frame); +private: + geometry_msgs::msg::TransformStamped lookup_transform( + std::string target_frame, + std::string source_frame); - visualization_msgs::msg::Marker create_lookahead_circle( - double x, double y, double radius, - std::string frame_id); + visualization_msgs::msg::Marker create_lookahead_circle( + double x, double y, double radius, + std::string frame_id); - void publishZeroVelocity(); + void publishZeroVelocity(); - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &uuid, - std::shared_ptr goal); + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); - void handle_accepted( - const std::shared_ptr> goal_handle); + void handle_accepted( + const std::shared_ptr> goal_handle); - void execute( - const std::shared_ptr> goal_handle); + void execute( + const std::shared_ptr> goal_handle); - /** - * @brief Handle the costmap data - * @param msg The costmap data - */ - void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /** + * @brief Handle the costmap data + * @param msg The costmap data + */ + void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); - int getCost(const nav_msgs::msg::OccupancyGrid &costmap, double x, double y); + int getCost(const nav_msgs::msg::OccupancyGrid & costmap, double x, double y); - nav_msgs::msg::OccupancyGrid current_costmap_; - rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Publisher::SharedPtr carrot_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; - rclcpp_action::Server::SharedPtr follow_path_server_; - rclcpp::Subscription::SharedPtr odom_sub_; + nav_msgs::msg::OccupancyGrid current_costmap_; + rclcpp::Subscription::SharedPtr costmap_subscriber_; + rclcpp::Publisher::SharedPtr carrot_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; + rclcpp_action::Server::SharedPtr follow_path_server_; + rclcpp::Subscription::SharedPtr odom_sub_; - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; - rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; - geometry_msgs::msg::PoseStamped current_pose_; - bool stamped_; - }; + geometry_msgs::msg::PoseStamped current_pose_; + bool stamped_; +}; } // namespace follower_action_server #endif // FOLLOWER_ACTION_SERVER_HPP_ From 78a839a7984f5ff640d21a634d38e9856ade3fb8 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Sat, 1 Jun 2024 14:08:25 -0700 Subject: [PATCH 03/11] More changes, working better --- urc_behaviors/src/search_for_aruco.cpp | 21 +++++++++++++------ .../launch/bringup_simulation.launch.py | 16 ++++++++++++-- .../path_planning/include/planner_server.hpp | 7 ------- .../path_planning/src/planner_server.cpp | 15 ------------- .../include/follower_action_server.hpp | 7 +++++++ .../src/follower_action_server.cpp | 15 +++++++++++++ 6 files changed, 51 insertions(+), 30 deletions(-) diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index 75164a40..06c3d82c 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -20,6 +20,14 @@ SearchForAruco::SearchForAruco(const rclcpp::NodeOptions & options) void SearchForAruco::search() { + // Wait until base_link to map transform is available + while (!tf_buffer_->canTransform("map", "base_link", tf2::TimePointZero)) { + RCLCPP_INFO(this->get_logger(), "Waiting for transform from base_link to map..."); + rclcpp::sleep_for(std::chrono::seconds(1)); + } + + RCLCPP_INFO(this->get_logger(), "Transform from base_link to map is available."); + // Generate a search path auto path = generate_search_path(0.1); @@ -77,14 +85,15 @@ void SearchForAruco::feedback_callback( nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) { + rclcpp::Time now = this->now(); nav_msgs::msg::Path path; path.header.frame_id = "map"; - path.header.stamp = this->now(); + path.header.stamp = now; // Lookup transform from base_link to map geometry_msgs::msg::TransformStamped transform; try { - transform = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + transform = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(this->get_logger(), "Failed to lookup transform: %s", ex.what()); throw std::runtime_error("Failed to lookup transform."); @@ -92,17 +101,17 @@ nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) for (double t = 0; t < 12 * M_PI; t += M_PI / 4) { geometry_msgs::msg::PoseStamped pose; - pose.header = path.header; + pose.header.frame_id = "map"; + pose.header.stamp = now; pose.pose.position.x = spiral_coeff * t * cos(t); pose.pose.position.y = spiral_coeff * t * sin(t); pose.pose.orientation.w = 1.0; // Transform pose to map frame - geometry_msgs::msg::PoseStamped transformed_pose; - tf2::doTransform(pose, transformed_pose, transform); + tf2::doTransform(pose.pose, pose.pose, transform); - path.poses.push_back(transformed_pose); + path.poses.push_back(pose); } return path; diff --git a/urc_bringup/launch/bringup_simulation.launch.py b/urc_bringup/launch/bringup_simulation.launch.py index 54782ed1..4a5d45bb 100644 --- a/urc_bringup/launch/bringup_simulation.launch.py +++ b/urc_bringup/launch/bringup_simulation.launch.py @@ -161,6 +161,12 @@ def generate_launch_description(): ], ) + search_for_aruco_node = Node( + package="urc_behaviors", + executable="urc_behaviors_SearchForAruco", + output="screen", + ) + map_to_odom_transform_node = Node( package="tf2_ros", executable="static_transform_publisher", @@ -185,7 +191,6 @@ def generate_launch_description(): on_exit=[ load_joint_state_broadcaster, load_drivetrain_controller, - map_to_odom_launch, ], ) ), @@ -198,9 +203,17 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessStart( target_action=elevation_mapping_node, + on_start=[map_to_odom_transform_node], + ) + ), + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=map_to_odom_transform_node, on_start=[ + map_to_odom_launch, path_planning_launch, trajectory_following_launch, + search_for_aruco_node, # bt_launch, ], ) @@ -209,6 +222,5 @@ def generate_launch_description(): gazebo, load_robot_state_publisher, spawn_robot, - map_to_odom_transform_node, ] ) diff --git a/urc_navigation/path_planning/include/planner_server.hpp b/urc_navigation/path_planning/include/planner_server.hpp index 7ec00b0a..8b0732c9 100644 --- a/urc_navigation/path_planning/include/planner_server.hpp +++ b/urc_navigation/path_planning/include/planner_server.hpp @@ -24,12 +24,6 @@ class PlannerServer : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * @brief Publish the plan to the /path topic for *visualization* purposes. The plan will be returned as a response to the service call. - * @param plan The plan to be published - */ - void publishPlan(const nav_msgs::msg::Path & plan); - /** * @brief Wait for the costmap to be available */ @@ -43,7 +37,6 @@ class PlannerServer : public rclcpp::Node nav_msgs::msg::OccupancyGrid current_costmap_; rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Publisher::SharedPtr plan_publisher_; rclcpp::Service::SharedPtr plan_service_; }; } // namespace planner_server diff --git a/urc_navigation/path_planning/src/planner_server.cpp b/urc_navigation/path_planning/src/planner_server.cpp index 5927b776..455418f3 100644 --- a/urc_navigation/path_planning/src/planner_server.cpp +++ b/urc_navigation/path_planning/src/planner_server.cpp @@ -16,11 +16,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) "plan", std::bind(&PlannerServer::generatePlan, this, std::placeholders::_1, std::placeholders::_2)); - // Create the publisher - plan_publisher_ = create_publisher( - "/path", - rclcpp::SystemDefaultsQoS()); - // Setup the costmap costmap_subscriber_ = create_subscription( "/costmap", @@ -71,21 +66,11 @@ void PlannerServer::generatePlan( response->error_code = urc_msgs::srv::GeneratePlan::Response::SUCCESS; RCLCPP_INFO(get_logger(), "Returning plan with %ld poses", plan.poses.size()); - - publishPlan(plan); } catch (const std::exception & e) { RCLCPP_ERROR(get_logger(), "Error generating plan: %s", e.what()); response->error_code = urc_msgs::srv::GeneratePlan::Response::FAILURE; } } - -void PlannerServer::publishPlan(const nav_msgs::msg::Path & plan) -{ - auto msg = std::make_unique(plan); - if (plan_publisher_->get_subscription_count() > 0) { - plan_publisher_->publish(std::move(msg)); - } -} } // namespace planner_server #include diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 98382c10..0bd68dac 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -53,11 +53,18 @@ class FollowerActionServer : public rclcpp::Node */ void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /** + * @brief Publish the plan to the /path topic for *visualization* purposes. The plan will be returned as a response to the service call. + * @param plan The plan to be published + */ + void publishPlan(const nav_msgs::msg::Path & plan); + int getCost(const nav_msgs::msg::OccupancyGrid & costmap, double x, double y); nav_msgs::msg::OccupancyGrid current_costmap_; rclcpp::Subscription::SharedPtr costmap_subscriber_; rclcpp::Publisher::SharedPtr carrot_pub_; + rclcpp::Publisher::SharedPtr plan_publisher_; rclcpp::Publisher::SharedPtr cmd_vel_pub_; rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; rclcpp_action::Server::SharedPtr follow_path_server_; diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 0d1f3433..4910d87d 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -43,6 +43,11 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) carrot_pub_ = create_publisher("carrot", 10); marker_pub_ = create_publisher("lookahead_circle", 10); + // Create the publisher + plan_publisher_ = create_publisher( + "/path", + rclcpp::SystemDefaultsQoS()); + odom_sub_ = create_subscription( get_parameter("odom_topic").as_string(), 10, @@ -196,6 +201,8 @@ void FollowerActionServer::execute( auto result = std::make_shared(); auto & path = goal_handle->get_goal()->path; + publishPlan(path); + // Create a PurePursuit object pure_pursuit::PurePursuitParams params; params.lookahead_distance = get_parameter("lookahead_distance").as_double(); @@ -264,6 +271,14 @@ void FollowerActionServer::execute( publishZeroVelocity(); } +void FollowerActionServer::publishPlan(const nav_msgs::msg::Path & plan) +{ + auto msg = std::make_unique(plan); + if (plan_publisher_->get_subscription_count() > 0) { + plan_publisher_->publish(std::move(msg)); + } +} + } // namespace follower_node #include From dee28e199da8238c986d27d5a5b5d7bc523e52cd Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Fri, 11 Apr 2025 19:17:06 -0400 Subject: [PATCH 04/11] Handle the case where goal type is GPS by returning success immediately --- external/ublox_dgnss | 1 + external/vectornav | 1 + urc_behaviors/src/search_for_aruco.cpp | 7 +++++++ 3 files changed, 9 insertions(+) create mode 160000 external/ublox_dgnss create mode 160000 external/vectornav diff --git a/external/ublox_dgnss b/external/ublox_dgnss new file mode 160000 index 00000000..1af51879 --- /dev/null +++ b/external/ublox_dgnss @@ -0,0 +1 @@ +Subproject commit 1af51879a6e61c26c75a9c7132eee4798d5bddae diff --git a/external/vectornav b/external/vectornav new file mode 160000 index 00000000..2859fe18 --- /dev/null +++ b/external/vectornav @@ -0,0 +1 @@ +Subproject commit 2859fe18a3c4a5e6d3dafcef9923df9a854a2660 diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index 06c3d82c..e2578f81 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -35,6 +35,13 @@ void SearchForAruco::search() auto goal = FollowPath::Goal(); goal.path = path; + // Handle the case where the goal type is GPS, so return success immediately + if (goal.goal_type == urc_msgs::action::SearchAruco::GPS) { + auto result = std::make_shared(); + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = std::bind( &SearchForAruco::goal_response_callback, From 844e673e734a98b39a794fc1a03096c076918e07 Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Sun, 13 Apr 2025 18:46:39 -0400 Subject: [PATCH 05/11] Added action definition for SearchAruco --- urc_msgs/action/SearchAruco.action | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 urc_msgs/action/SearchAruco.action diff --git a/urc_msgs/action/SearchAruco.action b/urc_msgs/action/SearchAruco.action new file mode 100644 index 00000000..8de93033 --- /dev/null +++ b/urc_msgs/action/SearchAruco.action @@ -0,0 +1,16 @@ +# request definition +uint16 ARUCO=0 +uint16 MALLET=1 +uint16 WATER_BOTTLE=2 +uint16 GPS=3 +uint16 goal_type +--- +# response definition +uint16 SUCCESS=0 +uint16 FAILURE=1 + +std_msgs/UInt32 result +uint16 error_code +--- +# feedback definition +float32 percentage_searched \ No newline at end of file From 69c0c81546929a31e67657026e611ff19790ccdb Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Sun, 13 Apr 2025 19:08:11 -0400 Subject: [PATCH 06/11] added action definition to CMakeLists --- urc_msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/urc_msgs/CMakeLists.txt b/urc_msgs/CMakeLists.txt index 0f8a8901..16e4f917 100644 --- a/urc_msgs/CMakeLists.txt +++ b/urc_msgs/CMakeLists.txt @@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/GridLocation.msg" "srv/GeneratePlan.srv" "action/FollowPath.action" + "action/SearchAruco.action" "srv/UpdateBehaviorTree.srv" DEPENDENCIES From 2e4fcb8ffcafdbcd4651ae10e546f8132422a318 Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Fri, 18 Apr 2025 19:22:59 -0400 Subject: [PATCH 07/11] Preliminary creation of search aruco tag behavior --- urc_behaviors/include/search_for_aruco.hpp | 16 ++++++++++++ urc_behaviors/src/search_for_aruco.cpp | 29 ++++++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/urc_behaviors/include/search_for_aruco.hpp b/urc_behaviors/include/search_for_aruco.hpp index 1141af66..2318c780 100644 --- a/urc_behaviors/include/search_for_aruco.hpp +++ b/urc_behaviors/include/search_for_aruco.hpp @@ -12,6 +12,9 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" +#include +#include + namespace urc_behaviors { class SearchForAruco : public rclcpp::Node @@ -43,6 +46,19 @@ class SearchForAruco : public rclcpp::Node std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; }; + +class ArucoSeen : public rclcpp::Node +{ +public: + ArucoSeen(const rclcpp::NodeOptions & options); + +private: + void aruco_callback(const std_msgs::msg::Bool::SharedPtr msg); + bool aruco_seen_ = false; + + rclcpp::Subscription::SharedPtr aruco_seen_subscriber_; + rclcpp::Publisher::SharedPtr aruco_seen_publisher_; +}; } // namespace urc_behaviors #endif // SEARCH_FOR_ARUCO_HPP_ diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index e2578f81..9357ae10 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -1,5 +1,6 @@ #include "search_for_aruco.hpp" #include +#include "urc_msgs/action/search_aruco.hpp" namespace urc_behaviors { @@ -42,6 +43,15 @@ void SearchForAruco::search() RCLCPP_INFO(this->get_logger(), "Goal succeeded"); } + aruco_seen_subscriber_ = this->create_subscription( + "/aruco_poses", 10, + std::bind(&ArucoSeen::aruco_callback, this, std::placeholders::_1)); + + aruco_seen_publisher_ = this->create_publisher( + "/aruco_seen", 10); + + RCLCPP_INFO(this->get_logger(), "Aruco seen node has been started."); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = std::bind( &SearchForAruco::goal_response_callback, @@ -90,6 +100,20 @@ void SearchForAruco::feedback_callback( RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance_to_goal); } +void aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg) { + bool aruco_seen = !msg->poses.empty(); + std_msgs::msg::Bool aruco_seen_msg; + aruco_seen_msg.data = aruco_seen; + + aruco_seen_publisher_->publish(aruco_seen_msg); + + if (aruco_seen) { + RCLCPP_INFO(this->get_logger(), "Aruco marker seen!"); + } else { + RCLCPP_INFO(this->get_logger(), "Aruco marker not seen."); + } +} + nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) { rclcpp::Time now = this->now(); @@ -123,7 +147,12 @@ nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) return path; } + +rclcpp::Subscription::SharedPtr aruco_seen_subscriber_; +rclcpp::Publisher::SharedPtr aruco_seen_publisher_; + } // namespace urc_behaviors #include RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::SearchForAruco) +RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::ArucoSeen) \ No newline at end of file From fea698ec0eb6708ef5491b852b6c96c4c7664f51 Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Sun, 20 Apr 2025 17:17:53 -0400 Subject: [PATCH 08/11] Modify FollowPath action definition to include goal type and pose --- urc_behaviors/include/search_for_aruco.hpp | 2 +- urc_behaviors/src/search_for_aruco.cpp | 14 ++++---------- urc_msgs/action/FollowPath.action | 3 ++- 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/urc_behaviors/include/search_for_aruco.hpp b/urc_behaviors/include/search_for_aruco.hpp index 2318c780..b258f2ae 100644 --- a/urc_behaviors/include/search_for_aruco.hpp +++ b/urc_behaviors/include/search_for_aruco.hpp @@ -28,7 +28,7 @@ class SearchForAruco : public rclcpp::Node ~SearchForAruco() = default; private: - void search(); + void search(const std::shared_ptr> goal_handle); nav_msgs::msg::Path generate_search_path(double spiral_coeff); diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index 9357ae10..865bb849 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -19,7 +19,7 @@ SearchForAruco::SearchForAruco(const rclcpp::NodeOptions & options) search(); } -void SearchForAruco::search() +void SearchForAruco::search(const std::shared_ptr> goal_handle) { // Wait until base_link to map transform is available while (!tf_buffer_->canTransform("map", "base_link", tf2::TimePointZero)) { @@ -45,7 +45,7 @@ void SearchForAruco::search() aruco_seen_subscriber_ = this->create_subscription( "/aruco_poses", 10, - std::bind(&ArucoSeen::aruco_callback, this, std::placeholders::_1)); + std::bind(&SearchForAruco::aruco_callback, this, std::placeholders::_1)); aruco_seen_publisher_ = this->create_publisher( "/aruco_seen", 10); @@ -100,7 +100,7 @@ void SearchForAruco::feedback_callback( RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance_to_goal); } -void aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg) { +void SearchForAruco::aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg) { bool aruco_seen = !msg->poses.empty(); std_msgs::msg::Bool aruco_seen_msg; aruco_seen_msg.data = aruco_seen; @@ -147,12 +147,6 @@ nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) return path; } - -rclcpp::Subscription::SharedPtr aruco_seen_subscriber_; -rclcpp::Publisher::SharedPtr aruco_seen_publisher_; - } // namespace urc_behaviors -#include -RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::SearchForAruco) -RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::ArucoSeen) \ No newline at end of file +#include \ No newline at end of file diff --git a/urc_msgs/action/FollowPath.action b/urc_msgs/action/FollowPath.action index 391dd54d..65ac1a01 100644 --- a/urc_msgs/action/FollowPath.action +++ b/urc_msgs/action/FollowPath.action @@ -1,13 +1,14 @@ # request definition nav_msgs/Path path +uint16 goal_type --- # response definition uint16 SUCCESS=0 uint16 FAILURE=1 uint16 OBSTACLE_DETECTED=2 -std_msgs/Empty result uint16 error_code +geometry_msgs/Pose result --- # feedback definition float32 distance_to_goal \ No newline at end of file From cfce915860d9536a02cb029f0827ad91f164e74e Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Sun, 20 Apr 2025 19:31:46 -0400 Subject: [PATCH 09/11] added both action client and action server functionality to search_for_aruco.cpp --- urc_behaviors/include/search_for_aruco.hpp | 25 ++++++----- urc_behaviors/src/search_for_aruco.cpp | 52 +++++++++++++++++++--- 2 files changed, 59 insertions(+), 18 deletions(-) diff --git a/urc_behaviors/include/search_for_aruco.hpp b/urc_behaviors/include/search_for_aruco.hpp index b258f2ae..878a207b 100644 --- a/urc_behaviors/include/search_for_aruco.hpp +++ b/urc_behaviors/include/search_for_aruco.hpp @@ -8,6 +8,7 @@ #include #include +#include #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" @@ -21,14 +22,16 @@ class SearchForAruco : public rclcpp::Node { public: using FollowPath = urc_msgs::action::FollowPath; + using SearchAruco = urc_msgs::action::SearchAruco; using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle; + using GoalHandleSearchAruco = rclcpp_action::ServerGoalHandle; explicit SearchForAruco(const rclcpp::NodeOptions & options); ~SearchForAruco() = default; private: - void search(const std::shared_ptr> goal_handle); + void search(const std::shared_ptr goal_handle); nav_msgs::msg::Path generate_search_path(double spiral_coeff); @@ -42,23 +45,21 @@ class SearchForAruco : public rclcpp::Node bool aruco_seen_; rclcpp::Time aruco_first_seen_time_; + void aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg); + void handle_accepted(const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; -}; - -class ArucoSeen : public rclcpp::Node -{ -public: - ArucoSeen(const rclcpp::NodeOptions & options); - -private: - void aruco_callback(const std_msgs::msg::Bool::SharedPtr msg); - bool aruco_seen_ = false; - + rclcpp_action::Server::SharedPtr search_server_; rclcpp::Subscription::SharedPtr aruco_seen_subscriber_; rclcpp::Publisher::SharedPtr aruco_seen_publisher_; }; + } // namespace urc_behaviors #endif // SEARCH_FOR_ARUCO_HPP_ diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index 865bb849..cfa0a22e 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -12,14 +12,26 @@ SearchForAruco::SearchForAruco(const rclcpp::NodeOptions & options) tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + search_server_ = rclcpp_action::create_server( + this, + "search_for_aruco", + std::bind( + &SearchForAruco::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind( + &SearchForAruco::handle_cancel, this, + std::placeholders::_1), + std::bind( + &SearchForAruco::handle_accepted, this, + std::placeholders::_1) + ); + follow_path_client_ = rclcpp_action::create_client( this, "follow_path"); - - search(); } -void SearchForAruco::search(const std::shared_ptr> goal_handle) +void SearchForAruco::search(const std::shared_ptr goal_handle) { // Wait until base_link to map transform is available while (!tf_buffer_->canTransform("map", "base_link", tf2::TimePointZero)) { @@ -37,15 +49,18 @@ void SearchForAruco::search(const std::shared_ptrget_goal()->goal_type == urc_msgs::action::SearchAruco::Goal::GPS) { auto result = std::make_shared(); goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + RCLCPP_INFO(this->get_logger(), "Goal succeeded immediately"); + return; } aruco_seen_subscriber_ = this->create_subscription( "/aruco_poses", 10, - std::bind(&SearchForAruco::aruco_callback, this, std::placeholders::_1)); + [this](geometry_msgs::msg::PoseArray::SharedPtr msg) { + this->aruco_callback(msg); + }); aruco_seen_publisher_ = this->create_publisher( "/aruco_seen", 10); @@ -147,6 +162,31 @@ nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff) return path; } + +void SearchForAruco::handle_accepted(const std::shared_ptr> goal_handle) { + std::thread{std::bind( + &SearchForAruco::search, this, + goal_handle)}.detach(); +} + +rclcpp_action::GoalResponse SearchForAruco::handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), "Received goal request"); + (void)uuid; + (void)goal; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse SearchForAruco::handle_cancel( + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + } // namespace urc_behaviors #include \ No newline at end of file From 239993377f2d751a14ebf4db83891dd344b3afd2 Mon Sep 17 00:00:00 2001 From: Varun Warrier Date: Sun, 20 Apr 2025 19:54:01 -0400 Subject: [PATCH 10/11] added aruco detection stop potentially --- drone_ws/src | 1 + rover_ws/src | 1 + .../include/follower_action_server.hpp | 6 ++++++ .../src/follower_action_server.cpp | 19 ++++++++++++++++++- 4 files changed, 26 insertions(+), 1 deletion(-) create mode 160000 drone_ws/src create mode 160000 rover_ws/src diff --git a/drone_ws/src b/drone_ws/src new file mode 160000 index 00000000..6c9596e6 --- /dev/null +++ b/drone_ws/src @@ -0,0 +1 @@ +Subproject commit 6c9596e6686e47a5e0483e65710ac22fcd961f69 diff --git a/rover_ws/src b/rover_ws/src new file mode 160000 index 00000000..51d5dbe5 --- /dev/null +++ b/rover_ws/src @@ -0,0 +1 @@ +Subproject commit 51d5dbe59d5242fcf6c2c5f7d698452652ffde4c diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 0bd68dac..18b761aa 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -31,7 +31,12 @@ class FollowerActionServer : public rclcpp::Node visualization_msgs::msg::Marker create_lookahead_circle( double x, double y, double radius, std::string frame_id); + + geometry_msgs::msg::PoseStamped current_aruco_pose_; + bool aruco_detected_{false}; + + void publishZeroVelocity(); rclcpp_action::GoalResponse handle_goal( @@ -69,6 +74,7 @@ class FollowerActionServer : public rclcpp::Node rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; rclcpp_action::Server::SharedPtr follow_path_server_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr aruco_sub_; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 55c54852..4cf25fc9 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -4,6 +4,8 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/exceptions.h" #include +#include + namespace follower_action_server { @@ -23,6 +25,8 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + + geometry_msgs::msg::PoseStamped current_aruco_pose_; stamped_ = get_parameter("cmd_vel_stamped").as_bool(); @@ -60,6 +64,19 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) current_pose_ = pose; }); + aruco_sub = create_subscription( + get_parameter("aruco_topic").as_string(), + 10, + [this](const geometry_msgs::msg::PoseArray::SharedPtr msg) { + if (msg->poses.empty()) { + return; + } + current_aruco_pose_.header = msg->header; + current_aruco_pose_.pose = msg->poses.front(); + aruco_detected_ = true; + } + ); + // Setup the costmap costmap_subscriber_ = create_subscription( "/costmap", @@ -220,7 +237,7 @@ void FollowerActionServer::execute( 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()) { + } else if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double() || aruco_detected_) { result->error_code = urc_msgs::action::FollowPath::Result::SUCCESS; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal has been reached!"); From ef7d030df342bd8395de67429aeb7dcf89c1c348 Mon Sep 17 00:00:00 2001 From: Rick Lee Date: Wed, 23 Apr 2025 17:33:54 -0400 Subject: [PATCH 11/11] Added functionality to preemptively cancel current goal upon detecting Aruco in search_aruco.cpp --- urc_behaviors/include/search_for_aruco.hpp | 5 +- urc_behaviors/src/search_for_aruco.cpp | 68 ++++++++++++++++--- .../include/follower_action_server.hpp | 1 + .../src/follower_action_server.cpp | 13 ---- 4 files changed, 63 insertions(+), 24 deletions(-) diff --git a/urc_behaviors/include/search_for_aruco.hpp b/urc_behaviors/include/search_for_aruco.hpp index 878a207b..b4b615cd 100644 --- a/urc_behaviors/include/search_for_aruco.hpp +++ b/urc_behaviors/include/search_for_aruco.hpp @@ -18,7 +18,7 @@ namespace urc_behaviors { -class SearchForAruco : public rclcpp::Node +class SearchForAruco : public rclcpp::Node, public std::enable_shared_from_this { public: using FollowPath = urc_msgs::action::FollowPath; @@ -43,7 +43,8 @@ class SearchForAruco : public rclcpp::Node rclcpp_action::Client::SharedPtr follow_path_client_; - bool aruco_seen_; + bool aruco_seen_ = false; + bool cancel_requested_ = false; rclcpp::Time aruco_first_seen_time_; void aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg); void handle_accepted(const std::shared_ptr goal_handle); diff --git a/urc_behaviors/src/search_for_aruco.cpp b/urc_behaviors/src/search_for_aruco.cpp index cfa0a22e..a1d026a3 100644 --- a/urc_behaviors/src/search_for_aruco.cpp +++ b/urc_behaviors/src/search_for_aruco.cpp @@ -63,7 +63,7 @@ void SearchForAruco::search(const std::shared_ptr goal_ha }); aruco_seen_publisher_ = this->create_publisher( - "/aruco_seen", 10); + "/aruco_seen_", 10); RCLCPP_INFO(this->get_logger(), "Aruco seen node has been started."); @@ -78,7 +78,57 @@ void SearchForAruco::search(const std::shared_ptr goal_ha &SearchForAruco::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); - follow_path_client_->async_send_goal(goal, send_goal_options); + auto goal_handle_future = follow_path_client_->async_send_goal( + goal, send_goal_options); + + if (rclcpp::spin_until_future_complete(std::enable_shared_from_this::shared_from_this(), goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "Failed to send goal to follow_path action server"); + return; + } + + rclcpp_action::ClientGoalHandle::SharedPtr follow_path_goal_handle_ = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Failed to receive goal handle"); + return; + } + + auto result_future = follow_path_client_->async_get_result(follow_path_goal_handle_); + auto wait_result = rclcpp::spin_until_future_complete(std::enable_shared_from_this::shared_from_this(), result_future, std::chrono::seconds(3)); + auto result = result_future.get(); + + if (rclcpp::FutureReturnCode::TIMEOUT == wait_result) { + RCLCPP_ERROR(this->get_logger(), "Timed out waiting for result"); + auto cancel_result_future = follow_path_client_->async_cancel_goal(follow_path_goal_handle_); + if (rclcpp::spin_until_future_complete(std::enable_shared_from_this::shared_from_this(), cancel_result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Failed to cancel goal"); + return; + } + RCLCPP_INFO(this->get_logger(), "Goal is being canceled"); + } else if (rclcpp::FutureReturnCode::SUCCESS != wait_result) { + RCLCPP_ERROR(this->get_logger(), "Failed to get result"); + return; + } + + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + if (result.result->SUCCESS == urc_msgs::action::FollowPath::Result::SUCCESS) { + RCLCPP_INFO(this->get_logger(), "Aruco marker found, goal succeeded."); + follow_path_client_->async_cancel_goal(follow_path_goal_handle_); + } else { + RCLCPP_INFO(this->get_logger(), "Goal completed but aruco marker not seen."); + } + } else if (result.code == rclcpp_action::ResultCode::ABORTED) { + RCLCPP_INFO(this->get_logger(), "Goal was aborted"); + return; + } else if (result.code == rclcpp_action::ResultCode::CANCELED) { + RCLCPP_INFO(this->get_logger(), "Goal was canceled"); + return; + } + + auto result_msg = std::make_shared(); + goal_handle->succeed(result_msg); } void SearchForAruco::goal_response_callback(const GoalHandleFollowPath::SharedPtr & goal_handle) @@ -116,16 +166,17 @@ void SearchForAruco::feedback_callback( } void SearchForAruco::aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg) { - bool aruco_seen = !msg->poses.empty(); + bool aruco_seen_ = !msg->poses.empty(); std_msgs::msg::Bool aruco_seen_msg; - aruco_seen_msg.data = aruco_seen; + aruco_seen_msg.data = aruco_seen_; aruco_seen_publisher_->publish(aruco_seen_msg); - if (aruco_seen) { - RCLCPP_INFO(this->get_logger(), "Aruco marker seen!"); - } else { - RCLCPP_INFO(this->get_logger(), "Aruco marker not seen."); + if (aruco_seen_ && !cancel_requested_) { + RCLCPP_INFO(this->get_logger(), "Aruco marker seen! Preempting follow_path."); + cancel_requested_ = true; + aruco_seen_ = true; + follow_path_client_->async_cancel_all_goals(); } } @@ -186,7 +237,6 @@ rclcpp_action::CancelResponse SearchForAruco::handle_cancel( (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } - } // namespace urc_behaviors #include \ No newline at end of file diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 18b761aa..1d8560b2 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -13,6 +13,7 @@ #include "tf2_ros/buffer.h" #include "urc_msgs/action/follow_path.hpp" #include +#include "geometry_msgs/msg/pose_array.hpp" namespace follower_action_server { diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 4cf25fc9..0beccbab 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -64,19 +64,6 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) current_pose_ = pose; }); - aruco_sub = create_subscription( - get_parameter("aruco_topic").as_string(), - 10, - [this](const geometry_msgs::msg::PoseArray::SharedPtr msg) { - if (msg->poses.empty()) { - return; - } - current_aruco_pose_.header = msg->header; - current_aruco_pose_.pose = msg->poses.front(); - aruco_detected_ = true; - } - ); - // Setup the costmap costmap_subscriber_ = create_subscription( "/costmap",