From 1041de9ee10cbd08bde67ed5fe16a0cf3dc43ef2 Mon Sep 17 00:00:00 2001 From: Yashas Ambati Date: Thu, 30 May 2024 11:09:31 -0700 Subject: [PATCH 1/3] 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 2/3] 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 3/3] 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