diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt index bf5c1967..dacf5f82 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt @@ -2,7 +2,22 @@ cmake_minimum_required(VERSION 3.5) project(uwrt_mars_rover_drivetrain_description) find_package(ament_cmake REQUIRED) -find_package(sensor_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_core REQUIRED) +find_package(geometry_msgs REQUIRED) + +set(dependencies + rclcpp + rclcpp_lifecycle + nav2_msgs + geometry_msgs + builtin_interfaces + nav2_core + pluginlib +) install( DIRECTORY @@ -27,4 +42,16 @@ if(BUILD_TESTING) ament_xmllint() endif() +# for temporary standalone executable, for navigating to a given point +include_directories(include) +add_executable(execute_trajectory src/execute_trajectory.cpp include/uwrt_mars_rover_drivetrain_description/execute_trajectory.hpp) +ament_target_dependencies(execute_trajectory ${dependencies}) +install(TARGETS + execute_trajectory + DESTINATION lib/${PROJECT_NAME}) + + ament_export_include_directories( + include +) + ament_package() diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml index 75746820..902906e5 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml @@ -7,25 +7,20 @@ controller_server: min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + goal_checker_plugins: ["simple_goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 + required_movement_radius: 0.1 movement_time_allowance: 10.0 # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True - general_goal_checker: + simple_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 + xy_goal_tolerance: 0.02 + yaw_goal_tolerance: 0.02 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -38,8 +33,6 @@ controller_server: min_speed_xy: 0.0 max_speed_xy: 0.26 min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 @@ -60,8 +53,8 @@ controller_server: critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 GoalAlign.scale: 24.0 + PathAlign.forward_point_distance: 0.1 GoalAlign.forward_point_distance: 0.1 PathDist.scale: 32.0 GoalDist.scale: 24.0 @@ -73,6 +66,7 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] + use_sim_time: True GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 @@ -119,7 +113,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: true - inflation_radius: 0.55 + inflation_radius: 0.7 cost_scaling_factor: 1.0 inflate_unknown: false inflate_around_unknown: true @@ -163,7 +157,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: true - inflation_radius: 0.55 + inflation_radius: 0.7 cost_scaling_factor: 1.0 inflate_unknown: false inflate_around_unknown: true @@ -176,7 +170,7 @@ map_saver: map_server: ros__parameters: - yaml_filename: "/home/meshva/uwrt/uwrt_mars_rover/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/map.yaml" + yaml_filename: "map.yaml" topic_name: "map" frame_id: "odom" diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/include/uwrt_mars_rover_drivetrain_description/execute_trajectory.hpp b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/include/uwrt_mars_rover_drivetrain_description/execute_trajectory.hpp new file mode 100644 index 00000000..7716c12b --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/include/uwrt_mars_rover_drivetrain_description/execute_trajectory.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "nav_msgs/msg/path.hpp" + +#include "geometry_msgs/msg/point_stamped.hpp" + +class ExecuteTrajectory : public rclcpp::Node +{ +public: + ExecuteTrajectory(); + +private: + rclcpp::Subscription::SharedPtr pointSubscriber_; + rclcpp_action::Client::SharedPtr pathClient_; + rclcpp_action::Client::SharedPtr followClient_; + + nav_msgs::msg::Path path_; + + void handleReceivedPoint(const geometry_msgs::msg::PointStamped point); + void pathGoalReceivedCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + void followGoalReceivedCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + void followGoalFeedbackCallback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); +}; diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py index fbb95edb..f4d02dd7 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py @@ -31,7 +31,9 @@ def generate_launch_description(): output='screen', respawn=True, respawn_delay=2.0, - parameters=[controller_yaml])] + parameters=[controller_yaml], + remappings=[ + ("/cmd_vel", "/differential_drivetrain_controller/cmd_vel_unstamped")])] nodes += [Node( package='nav2_planner', diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/rviz/urdf.rviz b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/rviz/urdf.rviz index 8f4b9f39..a59bc675 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/rviz/urdf.rviz +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/rviz/urdf.rviz @@ -3,6 +3,10 @@ Panels: Name: Displays - Class: rviz_common/Views Name: Views + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties Visualization Manager: Class: "" Displays: @@ -26,6 +30,14 @@ Visualization Manager: Name: root Tools: - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point Value: true Views: Current: diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/src/execute_trajectory.cpp b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/src/execute_trajectory.cpp new file mode 100644 index 00000000..ba3badc4 --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/src/execute_trajectory.cpp @@ -0,0 +1,117 @@ +#include "uwrt_mars_rover_drivetrain_description/execute_trajectory.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +ExecuteTrajectory::ExecuteTrajectory() : Node("execute_trajectory") +{ + pointSubscriber_ = this->create_subscription("clicked_point", 20, + std::bind(&ExecuteTrajectory::handleReceivedPoint, this, _1)); + + pathClient_ = rclcpp_action::create_client(this, "/compute_path_to_pose"); + followClient_ = rclcpp_action::create_client(this, "/follow_path"); + + // pathPublisher_ = this->create_publisher("/planned_path", 20); +} + +void ExecuteTrajectory::handleReceivedPoint(const geometry_msgs::msg::PointStamped point) +{ + RCLCPP_INFO(this->get_logger(), "*** GOT POINT %f, %f, %f", point.point.x, point.point.y, point.point.z); + + // SEND GOAL TO GET PATH + + // if action server not available, abort + if (!this->pathClient_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "ComputePathToPose action server not available after waiting"); + rclcpp::shutdown(); + } + + auto computePathMsg = nav2_msgs::action::ComputePathToPose::Goal(); + + computePathMsg.goal.pose.position.x = point.point.x; + computePathMsg.goal.pose.position.y = point.point.y; + computePathMsg.goal.pose.position.z = 0; + // [TODO] Specify Orientation - using default orientation for now + computePathMsg.planner_id = "GridBased"; + computePathMsg.goal.header.frame_id = "odom"; + computePathMsg.start.header.frame_id = "odom"; + + // send goal and attach callbacks + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + std::bind(&ExecuteTrajectory::pathGoalReceivedCallback, this, _1); + this->pathClient_->async_send_goal(computePathMsg, send_goal_options); +} + +void ExecuteTrajectory::pathGoalReceivedCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) +{ + RCLCPP_INFO(this->get_logger(), "path goal received"); + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + // publish resulting path for visualizing + path_ = result.result->path; + // pathPublisher_->publish(path_); // publish path to visualize + + // SEND GOAL TO FOLLOW PATH + + if (!this->followClient_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "FollowPath action server not available after waiting"); + rclcpp::shutdown(); + } + + auto followPathMsg = nav2_msgs::action::FollowPath::Goal(); + followPathMsg.path = path_; + followPathMsg.controller_id = "FollowPath"; + followPathMsg.goal_checker_id = "simple_goal_checker"; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + std::bind(&ExecuteTrajectory::followGoalReceivedCallback, this, _1); + send_goal_options.feedback_callback = + std::bind(&ExecuteTrajectory::followGoalFeedbackCallback, this, _1, _2); + this->followClient_->async_send_goal(followPathMsg, send_goal_options); +} + +void ExecuteTrajectory::followGoalFeedbackCallback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_INFO(this->get_logger(), "got feedback speed: %f, dist to goal: %f", feedback->speed, feedback->distance_to_goal); +} + +void ExecuteTrajectory::followGoalReceivedCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) +{ + RCLCPP_INFO(this->get_logger(), "follow goal received"); + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml index 1c0e4c4c..ded3fc33 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml @@ -38,6 +38,9 @@ ament_cmake_flake8 ament_cmake_xmllint + nav2_msgs + nav2_core + ament_cmake