diff --git a/mapping/CMakeLists.txt b/mapping/CMakeLists.txt new file mode 100644 index 00000000..fb0f8702 --- /dev/null +++ b/mapping/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.5) +project(mapping) + +include(../cmake/default_settings.cmake) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components 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_sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(PCL 1.10 REQUIRED) + +include_directories( + include + ${PCL_INCLUDE_DIRS} +) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +# Library creation +add_library(${PROJECT_NAME} SHARED + src/elevation_mapping.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} +) + +set(dependencies + rclcpp + rclcpp_components + std_msgs + sensor_msgs + geometry_msgs + tf2 + tf2_sensor_msgs + tf2_geometry_msgs + nav_msgs + pcl_conversions +) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) + +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "mapping::ElevationMapping" + EXECUTABLE ${PROJECT_NAME}_ElevationMapping +) + +# Install launch files. +install( + DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +# Install library +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_package() diff --git a/mapping/config/mapping_params.yaml b/mapping/config/mapping_params.yaml new file mode 100644 index 00000000..15c806e3 --- /dev/null +++ b/mapping/config/mapping_params.yaml @@ -0,0 +1,11 @@ +elevation_mapping: + ros__parameters: + map_frame: "map" + camera_frame: "camera_depth_frame" + resolution: 0.1 + width: 60 + depth_topic: "/depth_camera/points" + min_z: 0.1 + max_z: 1.0 + inflation_radius: 0.7 + inflate_obstacles: true diff --git a/mapping/include/elevation_mapping.hpp b/mapping/include/elevation_mapping.hpp new file mode 100644 index 00000000..a5e8ccad --- /dev/null +++ b/mapping/include/elevation_mapping.hpp @@ -0,0 +1,63 @@ +#ifndef ELEVATION_MAPPING_HPP_ +#define ELEVATION_MAPPING_HPP_ + +#include +#include +#include +#include +#include +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +namespace mapping +{ + +class ElevationMapping : public rclcpp::Node +{ +public: + explicit ElevationMapping(const rclcpp::NodeOptions & options); + ~ElevationMapping(); + +private: + void handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + + geometry_msgs::msg::TransformStamped lookup_transform( + std::string target_frame, + std::string source_frame, + rclcpp::Time time); + + bool worldToMap(double x, double y, nav_msgs::msg::MapMetaData info, std::pair & out); + + int cellDistance(double world_dist) + { + return std::ceil(world_dist / resolution_); + } + + void inflate(int cell_x, int cell_y, double cell_cost, int radius); + + double gaussian(double x); + + rclcpp::Subscription::SharedPtr depth_subscriber_; + rclcpp::Publisher::SharedPtr map_publisher_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + nav_msgs::msg::OccupancyGrid map_; + + int cell_inflation_radius_; + bool inflate_obstacles_; + + int8_t max_cost_ = 100; + + double resolution_; + double min_z_; + double max_z_; + unsigned int width_; + std::string map_frame_; + std::string camera_frame_; +}; + +} // namespace mapping + +#endif // ELEVATION_MAPPING_HPP_ diff --git a/mapping/launch/mapping.launch.py b/mapping/launch/mapping.launch.py new file mode 100644 index 00000000..d65f77a3 --- /dev/null +++ b/mapping/launch/mapping.launch.py @@ -0,0 +1,27 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + elevation_mapping_node = Node( + package="mapping", + executable="mapping_ElevationMapping", + output="screen", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("mapping"), + "config", + "mapping_params.yaml", + ] + ) + ], + ) + + return LaunchDescription( + [ + elevation_mapping_node, + ] + ) diff --git a/mapping/package.xml b/mapping/package.xml new file mode 100644 index 00000000..82912fb7 --- /dev/null +++ b/mapping/package.xml @@ -0,0 +1,30 @@ + + + + mapping + 0.0.0 + Convert point cloud from depth camera to elevation map + yambati03 + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_components + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + tf2 + tf2_sensor_msgs + tf2_geometry_msgs + pcl_conversions + pcl_1.10 + + + ament_cmake + + \ No newline at end of file diff --git a/mapping/src/elevation_mapping.cpp b/mapping/src/elevation_mapping.cpp new file mode 100644 index 00000000..4090bffe --- /dev/null +++ b/mapping/src/elevation_mapping.cpp @@ -0,0 +1,199 @@ +#include "elevation_mapping.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace mapping +{ + +ElevationMapping::ElevationMapping(const rclcpp::NodeOptions & options) +: Node("elevation_mapping", options) +{ + RCLCPP_INFO(this->get_logger(), "Mapping node has been started."); + + declare_parameter("map_frame", "odom"); + declare_parameter("camera_frame", "camera_depth_frame"); + declare_parameter("depth_topic", "/depth_camera/points"); + + declare_parameter("resolution", 0.1); + declare_parameter("width", 60); + declare_parameter("min_z", 0.1); + declare_parameter("max_z", 2.0); + + declare_parameter("inflation_radius", 0.1); + declare_parameter("inflate_obstacles", true); + + width_ = get_parameter("width").as_int(); + resolution_ = get_parameter("resolution").as_double(); + map_frame_ = get_parameter("map_frame").as_string(); + camera_frame_ = get_parameter("camera_frame").as_string(); + min_z_ = get_parameter("min_z").as_double(); + max_z_ = get_parameter("max_z").as_double(); + + cell_inflation_radius_ = cellDistance(get_parameter("inflation_radius").as_double()); + inflate_obstacles_ = get_parameter("inflate_obstacles").as_bool(); + + RCLCPP_INFO(this->get_logger(), "Cell inflation radius set to %d", cell_inflation_radius_); + RCLCPP_INFO( + this->get_logger(), "Inflate obstacles set to %s.", inflate_obstacles_ ? "true" : "false"); + + map_.header.frame_id = map_frame_; + map_.info.resolution = resolution_; + map_.info.width = map_.info.height = width_; + map_.data.resize(map_.info.width * map_.info.height); + + depth_subscriber_ = create_subscription( + get_parameter("depth_topic").as_string(), 10, + std::bind(&ElevationMapping::handlePointcloud, this, std::placeholders::_1)); + + map_publisher_ = create_publisher("/costmap", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +ElevationMapping::~ElevationMapping() +{ +} + +geometry_msgs::msg::TransformStamped ElevationMapping::lookup_transform( + std::string target_frame, + std::string source_frame, + rclcpp::Time time) +{ + geometry_msgs::msg::TransformStamped transform; + + try { + transform = tf_buffer_->lookupTransform(target_frame, source_frame, time); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what()); + } + + return transform; +} + +bool ElevationMapping::worldToMap( + double x, double y, nav_msgs::msg::MapMetaData info, + std::pair & out) +{ + if (x < info.origin.position.x || x >= info.origin.position.x + info.width * info.resolution || + y < info.origin.position.y || y >= info.origin.position.y + info.height * info.resolution) + { + return false; + } + + out.first = (x - info.origin.position.x) / info.resolution; + out.second = (y - info.origin.position.y) / info.resolution; + + return true; +} + +void ElevationMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + // Transform the point cloud to the map frame + auto camera_to_map = lookup_transform(map_frame_, msg->header.frame_id, msg->header.stamp); + sensor_msgs::msg::PointCloud2 cloud_global_; + tf2::doTransform(*msg, cloud_global_, camera_to_map); + + // Convert the transformed point cloud to a PCL point cloud + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(cloud_global_, *cloud); + + // Remove NaN values from the point cloud + std::vector indices; + pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); + + // Set the origin of the costmap to the current position of the robot + tf2::Stamped trans; + tf2::fromMsg(lookup_transform(map_frame_, "base_link", msg->header.stamp), trans); + + auto pos = trans.getOrigin(); + double x = static_cast(pos.x() / map_.info.resolution) * map_.info.resolution; + double y = static_cast(pos.y() / map_.info.resolution) * map_.info.resolution; + + map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5; + map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5; + map_.info.origin.position.z = 0.0; + map_.info.origin.orientation.w = 1.0; + + // Reset the costmap + std::fill(map_.data.begin(), map_.data.end(), 0); + + // Update the costmap with the point cloud + for (unsigned int i = 0; i < cloud->size(); i++) { + auto & point = cloud->points[i]; + + std::pair map_coord; + if (!worldToMap(point.x, point.y, map_.info, map_coord)) { + continue; + } + + double z = point.z - pos.z(); + double y = point.y - pos.y(); + double x = point.x - pos.x(); + + if (z < min_z_ || std::sqrt(x * x + y * y) > 2.8) { + continue; + } + + int costmap_index = map_coord.first + map_coord.second * map_.info.width; + + double cost = 0.0; + + if (z > max_z_) { + cost = max_cost_; + } else { + cost = (z - min_z_) / (max_z_ - min_z_) * max_cost_; + } + + if (cost > map_.data[costmap_index]) { + map_.data[costmap_index] = cost; + + if (inflate_obstacles_) { + inflate(map_coord.first, map_coord.second, cost, cell_inflation_radius_); + } + } + } + + map_.header.stamp = get_clock()->now(); + map_publisher_->publish(map_); +} + +void ElevationMapping::inflate(int cell_x, int cell_y, double cell_cost, int radius) +{ + for (int x = cell_x - radius; x <= cell_x + radius; x++) { + for (int y = cell_y - radius; y <= cell_y + radius; y++) { + if (x < 0 || x >= map_.info.width || y < 0 || y >= map_.info.height) { + continue; + } + + int dist = std::sqrt(std::pow(x - cell_x, 2) + std::pow(y - cell_y, 2)); + + if (dist <= radius) { + int index = x + y * map_.info.width; + double inflated_cost = gaussian(dist) * cell_cost; + + if (inflated_cost > map_.data[index]) { + map_.data[index] = inflated_cost; + } + } + } + } +} + +double ElevationMapping::gaussian(double x) +{ + return std::exp(-0.5 * x * x / cell_inflation_radius_); +} + +} // namespace mapping + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mapping::ElevationMapping) diff --git a/urc_bringup/config/controller_config.yaml b/urc_bringup/config/controller_config.yaml index 0234a16d..d1d23801 100644 --- a/urc_bringup/config/controller_config.yaml +++ b/urc_bringup/config/controller_config.yaml @@ -48,9 +48,9 @@ rover_drivetrain_controller: left_wheel_names: ["left_wheel"] right_wheel_names: ["right_wheel"] - wheel_separation: 0.50 + wheel_separation: 0.78 #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal - wheel_radius: 0.20 + wheel_radius: 0.12 wheel_separation_multiplier: 1.0 left_wheel_radius_multiplier: 1.0 @@ -75,8 +75,8 @@ rover_drivetrain_controller: linear.x.has_velocity_limits: true linear.x.has_acceleration_limits: true linear.x.has_jerk_limits: false - linear.x.max_velocity: 2.5 - linear.x.min_velocity: -2.5 + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 linear.x.max_acceleration: 2.5 linear.x.max_jerk: 0.0 linear.x.min_jerk: 0.0 diff --git a/urc_bringup/launch/bringup.launch.py b/urc_bringup/launch/bringup.launch.py index 00579afe..d8792314 100644 --- a/urc_bringup/launch/bringup.launch.py +++ b/urc_bringup/launch/bringup.launch.py @@ -42,6 +42,7 @@ def generate_launch_description(): xacro_file, mappings={"use_simulation": "false"} ) robot_desc = robot_description_config.toxml() + gps_config = os.path.join(get_package_share_directory("urc_bringup"), "config", "nmea_serial_driver.yaml") control_node = Node( package="controller_manager", @@ -111,6 +112,12 @@ def generate_launch_description(): ) ) + launch_gps = Node( + package='nmea_navsat_driver', + executable='nmea_serial_driver', + output='screen', + parameters=[gps_config]) + launch_imu = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_imu_driver, "launch", "imu_serial_driver.launch.py") diff --git a/urc_bringup/launch/bringup_simulation.launch.py b/urc_bringup/launch/bringup_simulation.launch.py index 46152fa4..fc8b291e 100644 --- a/urc_bringup/launch/bringup_simulation.launch.py +++ b/urc_bringup/launch/bringup_simulation.launch.py @@ -1,10 +1,13 @@ import os from xacro import process_file from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.actions import SetEnvironmentVariable, RegisterEventHandler -from launch.substitutions import LaunchConfiguration -from launch.event_handlers import OnProcessExit +from launch.actions import ( + IncludeLaunchDescription, + SetEnvironmentVariable, + RegisterEventHandler, +) +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit, OnProcessStart from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -17,6 +20,7 @@ def generate_launch_description(): pkg_urc_bringup = get_package_share_directory("urc_bringup") pkg_path_planning = get_package_share_directory("path_planning") pkg_trajectory_following = get_package_share_directory("trajectory_following") + pkg_urc_test = get_package_share_directory("urc_test") controller_config_file_dir = os.path.join( pkg_urc_bringup, "config", "ros2_control_walli.yaml" @@ -38,7 +42,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_gazebo_ros, "launch", "gazebo.launch.py"), ), - launch_arguments={"use_sim_time": "true", "world": world_path}.items(), + launch_arguments={"use_sim_time": "true"}.items(), # "world": world_path ) enable_color = SetEnvironmentVariable(name="RCUTILS_COLORIZED_OUTPUT", value="1") @@ -143,8 +147,35 @@ def generate_launch_description(): ) ) - dummy_costmap_publisher = Node( - package="urc_test", executable="costmap_generator", output="screen" + elevation_mapping_node = Node( + package="mapping", + executable="mapping_ElevationMapping", + output="screen", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("mapping"), + "config", + "mapping_params.yaml", + ] + ) + ], + ) + + map_to_odom_transform_node = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["10", "10", "0", "0", "0", "0", "map", "odom"], + ) + + map_to_odom_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + pkg_urc_test, + "launch", + "odom_to_map_pose.launch.py", + ) + ) ) return LaunchDescription( @@ -156,7 +187,20 @@ def generate_launch_description(): load_joint_state_broadcaster, load_drivetrain_controller, teleop_launch, - dummy_costmap_publisher, + map_to_odom_launch, + ], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_drivetrain_controller, + on_exit=[elevation_mapping_node], + ) + ), + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=elevation_mapping_node, + on_start=[ path_planning_launch, trajectory_following_launch, bt_launch, @@ -167,5 +211,6 @@ def generate_launch_description(): gazebo, load_robot_state_publisher, spawn_robot, + map_to_odom_transform_node, ] ) diff --git a/urc_bringup/launch/bt.launch.py b/urc_bringup/launch/bt.launch.py index fe46bb3a..df0fe7bb 100644 --- a/urc_bringup/launch/bt.launch.py +++ b/urc_bringup/launch/bt.launch.py @@ -14,6 +14,8 @@ def generate_launch_description(): "libbt_call_generate_plan.so", "libbt_call_trigger.so", "libbt_follow_path.so", + "libbt_update_current_pose.so", + "libbt_status_light_publisher.so" ] node_lib_path_base = os.path.join( Path(get_package_share_directory("urc_bt_nodes")).parent.parent.absolute(), diff --git a/urc_bringup/strategies/bt_test.xml b/urc_bringup/strategies/bt_test.xml index 46f599c1..845c9bb1 100644 --- a/urc_bringup/strategies/bt_test.xml +++ b/urc_bringup/strategies/bt_test.xml @@ -1,33 +1,34 @@ - - - - + + + - - - - - - - - - - - - - - + message="Trying to Plan Path..."/> + + + + + + + + + + + + + + + + @@ -39,11 +40,6 @@ - - Service name. - @@ -56,6 +52,16 @@ Message to log. + + + + + + + + diff --git a/urc_bringup/strategies/urc_strategies.btproj b/urc_bringup/strategies/urc_strategies.btproj index 9026220a..0203af58 100644 --- a/urc_bringup/strategies/urc_strategies.btproj +++ b/urc_bringup/strategies/urc_strategies.btproj @@ -34,6 +34,14 @@ + + + + + + + + diff --git a/urc_bt/src/bt_orchestrator.cpp b/urc_bt/src/bt_orchestrator.cpp index 176e1f51..9a19c4ab 100644 --- a/urc_bt/src/bt_orchestrator.cpp +++ b/urc_bt/src/bt_orchestrator.cpp @@ -27,8 +27,7 @@ namespace behavior BehaviorTreeOrchestrator::BehaviorTreeOrchestrator(const rclcpp::NodeOptions & options) : rclcpp::Node("bt_orchestrator", options) { - logger_ = - std::make_unique(rclcpp::get_logger("bt_orchestrator")); + logger_ = std::make_unique(rclcpp::get_logger("bt_orchestrator")); logger_->set_level(rclcpp::Logger::Level::Debug); declare_parameter>("normal_node_lib_dirs"); declare_parameter>("ros_node_lib_dirs"); @@ -66,12 +65,11 @@ BehaviorTreeOrchestrator::BehaviorTreeOrchestrator(const rclcpp::NodeOptions & o for (const auto & node_lib_dir : temp) { RCLCPP_INFO(*logger_, "\t%s", node_lib_dir.c_str()); - // tree_factory_.registerFromPlugin(node_lib_dir); RegisterRosNode(tree_factory_, node_lib_dir, params_); } } - // create default tree if a file is speficied + // create default tree if a file is specified if (has_parameter("tree_file_dir")) { std::string temp; get_parameter("tree_file_dir", temp); @@ -82,9 +80,8 @@ BehaviorTreeOrchestrator::BehaviorTreeOrchestrator(const rclcpp::NodeOptions & o tree_ = std::make_unique(tree_factory_.createTreeFromFile(temp)); } else { RCLCPP_WARN( - *logger_, "No behavior tree file set. Will not able to start " - "the orchestrator until upon calling service " - "/update_tree."); + *logger_, + "The behavior tree file is not set. Cannot start the orchestrator until the /update_tree service is called."); } if (has_parameter("tick_rate")) { tick_rate_ = get_parameter("tick_rate").as_int(); @@ -95,27 +92,31 @@ BehaviorTreeOrchestrator::BehaviorTreeOrchestrator(const rclcpp::NodeOptions & o // starts service update_bt_service_ = create_service( - "~/update_tree", // - [this]( // - const UpdateBTReqest request, UpdateBTResponse response) { // + "~/update_tree", + [this]( + const UpdateBTReqest request, UpdateBTResponse response) + { response->success = RenewTree( request->use_dir, request->tree_dir, request->tree_content); return response; }); + reload_bt_service_ = create_service( - "~/reload", [this](const TriggerRequest, TriggerResponse response) { + "~/reload", [this](const TriggerRequest, TriggerResponse response) + { if (tree_dir_ == nullptr) { - RCLCPP_WARN(*logger_, "Tree dir is null, cannot perform reload."); + RCLCPP_WARN(*logger_, "Tree directory is null, cannot perform reload."); response->success = false; } else { response->success = RenewTree(true, *tree_dir_, ""); } return response; }); + start_bt_service_ = create_service( - // - "~/start_bt", [this](const TriggerRequest, TriggerResponse response) { + "~/start_bt", [this](const TriggerRequest, TriggerResponse response) + { if (!is_running_) { response->success = Start(); } else { @@ -124,8 +125,10 @@ BehaviorTreeOrchestrator::BehaviorTreeOrchestrator(const rclcpp::NodeOptions & o } return response; }); + stop_bt_service_ = create_service( - "~/stop_bt", [this](TriggerRequest, TriggerResponse response) { + "~/stop_bt", [this](TriggerRequest, TriggerResponse response) + { if (is_running_) { response->success = Stop(); } else { @@ -135,7 +138,6 @@ BehaviorTreeOrchestrator::BehaviorTreeOrchestrator(const rclcpp::NodeOptions & o return response; }); - // if the tree is loaded, start the tree. if (is_tree_loaded()) { Initialize(); Start(); @@ -160,16 +162,16 @@ bool BehaviorTreeOrchestrator::RenewTree( } Stop(); - RCLCPP_DEBUG(*logger_, "Load tree successfull. Start hot swapping..."); + RCLCPP_DEBUG(*logger_, "Loaded new tree successfully, hot-swapping..."); tree_ = std::move(new_tree_); Initialize(); - Start(); // auto start - RCLCPP_DEBUG(*logger_, "Hot swapping successful!"); + Start(); // auto start + RCLCPP_DEBUG(*logger_, "Successfully hot-swapped the tree."); } catch (std::exception & e) { - RCLCPP_ERROR(*logger_, "Fail to load new tree. %s.", e.what()); + RCLCPP_ERROR(*logger_, "Failed to load new tree: %s.", e.what()); is_running_ = false; - tree_.reset(nullptr); // auto stop tree + tree_.reset(nullptr); // auto stop tree return false; } @@ -196,28 +198,30 @@ bool BehaviorTreeOrchestrator::Start() if (!is_tree_loaded()) { RCLCPP_ERROR( *logger_, - "Tree is not loaded! Not able to start the orchestrator."); + "No tree is loaded! Cannot start the orchestrator."); return false; } is_running_ = true; std::thread( - [this]() { - RCLCPP_DEBUG(*logger_, "Staring BT Orchestrator..."); + [this]() + { + RCLCPP_DEBUG(*logger_, "Starting BT orchestrator..."); rclcpp::Rate rate(tick_rate_); while (is_running_) { tree_->tickExactlyOnce(); rate.sleep(); } - }).detach(); + }) + .detach(); return true; } bool BehaviorTreeOrchestrator::Stop() { is_running_ = false; - RCLCPP_DEBUG(*logger_, "Stopping BT Orchestrator..."); + RCLCPP_DEBUG(*logger_, "Stopping BT orchestrator..."); return true; } diff --git a/urc_bt_nodes/CMakeLists.txt b/urc_bt_nodes/CMakeLists.txt index 41bb1d4b..17a1c667 100644 --- a/urc_bt_nodes/CMakeLists.txt +++ b/urc_bt_nodes/CMakeLists.txt @@ -98,6 +98,32 @@ install( LIBRARY DESTINATION lib ) +add_library( + bt_status_light_publisher SHARED + src/actions/status_light_publisher.cpp + include/urc_bt_nodes/actions/status_light_publisher.hpp +) +ament_target_dependencies(bt_status_light_publisher ${dependencies}) +target_compile_features(bt_status_light_publisher PUBLIC c_std_99 cxx_std_17) +target_compile_definitions(bt_status_light_publisher PRIVATE BT_PLUGIN_EXPORT) +install( + TARGETS bt_status_light_publisher + LIBRARY DESTINATION lib +) + +add_library( + bt_update_current_pose SHARED + src/subscribers/update_current_pose.cpp + include/urc_bt_nodes/subscribers/update_current_pose.hpp +) +ament_target_dependencies(bt_update_current_pose ${dependencies}) +target_compile_features(bt_update_current_pose PUBLIC c_std_99 cxx_std_17) +target_compile_definitions(bt_update_current_pose PRIVATE BT_PLUGIN_EXPORT) +install( + TARGETS bt_update_current_pose + LIBRARY DESTINATION lib +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp b/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp new file mode 100644 index 00000000..3bf6d6fe --- /dev/null +++ b/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp @@ -0,0 +1,35 @@ +#ifndef STATUS_LIGHT_PUBLISHER_HPP__ +#define STATUS_LIGHT_PUBLISHER_HPP__ + +#include "behaviortree_cpp/basic_types.h" +#include "behaviortree_cpp/tree_node.h" +#include "behaviortree_cpp/tree_node.h" +#include "behaviortree_ros2/ros_node_params.hpp" +#include "behaviortree_ros2/bt_topic_pub_node.hpp" +#include "string" +#include "std_msgs/msg/int8.hpp" + +namespace behavior::actions +{ +class StatusLightPublisher : public BT::RosTopicPubNode +{ +public: + StatusLightPublisher( + const std::string & instance_name, const BT::NodeConfig & conf, + const BT::RosNodeParams & params) + : BT::RosTopicPubNode(instance_name, conf, params) {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("value"), + }); + } + + bool setMessage(std_msgs::msg::Int8 & msg) override; + +}; +} + +#endif diff --git a/urc_bt_nodes/include/urc_bt_nodes/base/log_info.hpp b/urc_bt_nodes/include/urc_bt_nodes/base/log_info.hpp index ad3092db..8aa36878 100644 --- a/urc_bt_nodes/include/urc_bt_nodes/base/log_info.hpp +++ b/urc_bt_nodes/include/urc_bt_nodes/base/log_info.hpp @@ -21,8 +21,8 @@ class LogInfo : public BT::SyncActionNode inline static PortsList providedPorts() { return { - InputPort>("logger"), // - InputPort("message") // + InputPort>("logger"), + InputPort("message") }; } diff --git a/urc_bt_nodes/include/urc_bt_nodes/subscribers/update_current_pose.hpp b/urc_bt_nodes/include/urc_bt_nodes/subscribers/update_current_pose.hpp new file mode 100644 index 00000000..87147319 --- /dev/null +++ b/urc_bt_nodes/include/urc_bt_nodes/subscribers/update_current_pose.hpp @@ -0,0 +1,28 @@ +#ifndef UPDATE_CURRENT_POSE_HPP_ +#define UPDATE_CURRENT_POSE_HPP_ + +#include "behaviortree_ros2/bt_topic_sub_node.hpp" +#include +#include +#include + +using namespace BT; + +namespace behavior::subscribers +{ +class UpdateCurrentPose : public RosTopicSubNode +{ +public: + UpdateCurrentPose(const std::string & name, const NodeConfig & conf, const RosNodeParams & params) + : RosTopicSubNode(name, conf, params) {} + + static PortsList providedPorts() + { + return providedBasicPorts({OutputPort("pose")}); + } + + NodeStatus onTick(const std::shared_ptr & last_msg) override; +}; +} // namespace behavior::subscribers + +#endif /* UPDATE_CURRENT_POSE_HPP_ */ diff --git a/urc_bt_nodes/src/actions/call_generate_plan.cpp b/urc_bt_nodes/src/actions/call_generate_plan.cpp index 8662062e..562ba5ec 100644 --- a/urc_bt_nodes/src/actions/call_generate_plan.cpp +++ b/urc_bt_nodes/src/actions/call_generate_plan.cpp @@ -15,6 +15,10 @@ bool CallGeneratePlan::setRequest(typename Request::SharedPtr & request) auto start_pose = getInput("start_pose").value(); auto goal_pose = getInput("goal_pose").value(); + RCLCPP_INFO( + node_->get_logger(), "Calling service to generate plan from (%.2f, %.2f) to (%.2f, %.2f)", + start_pose.position.x, start_pose.position.y, goal_pose.position.x, goal_pose.position.y); + request->start = geometry_msgs::msg::PoseStamped(); request->start.pose = start_pose; request->goal.pose = goal_pose; @@ -32,11 +36,10 @@ BT::NodeStatus CallGeneratePlan::onResponseReceived(const typename Response::Sha } } -} // namespace behavior::actions +} // namespace behavior::actions namespace BT { - template<> inline geometry_msgs::msg::Pose convertFromString(StringView str) { @@ -48,11 +51,16 @@ inline geometry_msgs::msg::Pose convertFromString(StringView str) geometry_msgs::msg::Pose output; output.position.x = convertFromString(coordinates[0]); output.position.y = convertFromString(coordinates[1]); - output.orientation.z = convertFromString(coordinates[2]); + + // Convert theta to quaternion without tf2 + double theta = convertFromString(coordinates[2]); + output.orientation.x = 0.0; + output.orientation.y = 0.0; + output.orientation.z = std::sin(theta / 2.0); + output.orientation.w = std::cos(theta / 2.0); return output; } - -} // namespace BT +} // namespace BT CreateRosNodePlugin(behavior::actions::CallGeneratePlan, "CallGeneratePlan"); diff --git a/urc_bt_nodes/src/actions/follow_path.cpp b/urc_bt_nodes/src/actions/follow_path.cpp index 7519a51b..0e5e283a 100644 --- a/urc_bt_nodes/src/actions/follow_path.cpp +++ b/urc_bt_nodes/src/actions/follow_path.cpp @@ -25,14 +25,15 @@ BT::NodeStatus FollowPath::onFeedback(const std::shared_ptr feed return BT::NodeStatus::FAILURE; } RCLCPP_INFO( - node_->get_logger(), "Following Path, %.2f m away from destination.", + node_->get_logger(), "Following Path! %.2f m away from destination.", feedback->distance_to_goal); - return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::RUNNING; } BT::NodeStatus FollowPath::onResultReceived(const WrappedResult & wr) { - RCLCPP_INFO(node_->get_logger(), "Path following is finished, code: %hu.", wr.result->error_code); + RCLCPP_INFO( + node_->get_logger(), "Finished following path with error code: %hu.", wr.result->error_code); if (wr.result->error_code == 0) { return BT::NodeStatus::SUCCESS; } @@ -43,11 +44,11 @@ BT::NodeStatus FollowPath::onResultReceived(const WrappedResult & wr) BT::NodeStatus FollowPath::onFailure(BT::ActionNodeErrorCode error) { - RCLCPP_ERROR(node_->get_logger(), "BT Action Node Error: %u.", error); + RCLCPP_ERROR(node_->get_logger(), "%s: Failed with error %s", name().c_str(), toStr(error)); return BT::NodeStatus::FAILURE; } -} // namespace behavior::actions +} // namespace behavior::actions #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(behavior::actions::FollowPath, "FollowPath"); diff --git a/urc_bt_nodes/src/actions/status_light_publisher.cpp b/urc_bt_nodes/src/actions/status_light_publisher.cpp new file mode 100644 index 00000000..31b3075f --- /dev/null +++ b/urc_bt_nodes/src/actions/status_light_publisher.cpp @@ -0,0 +1,27 @@ +#include "urc_bt_nodes/actions/status_light_publisher.hpp" +#include "behaviortree_cpp/basic_types.h" +#include + +namespace behavior::actions +{ +bool StatusLightPublisher::setMessage(std_msgs::msg::Int8 & msg) +{ + msg = getInput("value").value(); + return true; +} +} + +namespace BT +{ +template<> +inline std_msgs::msg::Int8 convertFromString(StringView str) +{ + std_msgs::msg::Int8 output; + output.data = convertFromString(str); + return output; +} +} // namespace BT + + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(behavior::actions::StatusLightPublisher, "StatusLightPublisher"); diff --git a/urc_bt_nodes/src/subscribers/update_current_pose.cpp b/urc_bt_nodes/src/subscribers/update_current_pose.cpp new file mode 100644 index 00000000..c6c2c253 --- /dev/null +++ b/urc_bt_nodes/src/subscribers/update_current_pose.cpp @@ -0,0 +1,20 @@ +#include "urc_bt_nodes/subscribers/update_current_pose.hpp" + +using namespace BT; + +namespace behavior::subscribers +{ +NodeStatus UpdateCurrentPose::onTick( + const std::shared_ptr & last_msg) +{ + if (last_msg) { + RCLCPP_INFO(node_->get_logger(), "New current pose received!"); + setOutput("pose", last_msg->pose); + } + + return NodeStatus::SUCCESS; +} +} // namespace behavior::subscribers + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(behavior::subscribers::UpdateCurrentPose, "UpdateCurrentPose"); diff --git a/urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp b/urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp index fc3f3aa5..4396d588 100644 --- a/urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp +++ b/urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp @@ -48,7 +48,7 @@ class RoverDrivetrain : public hardware_interface::SystemInterface // basic info const std::string hardware_interface_name; double publish_encoder_ticks_frequency_; - static constexpr int ENCODER_CPR = 2048; + static constexpr int ENCODER_CPR = 42 * 4; static constexpr float WHEEL_RADIUS = 0.170; static constexpr float VEL_TO_COUNTS_FACTOR = ENCODER_CPR / (2 * M_PI * WHEEL_RADIUS); static constexpr int QPPR = 15562; diff --git a/urc_hw_description/urdf/walli_sensors.xacro b/urc_hw_description/urdf/walli_sensors.xacro index 1d4663b5..8a9ebeca 100644 --- a/urc_hw_description/urdf/walli_sensors.xacro +++ b/urc_hw_description/urdf/walli_sensors.xacro @@ -291,8 +291,8 @@ 1.51844 - 1280 - 720 + 640 + 360 R8G8B8 diff --git a/urc_msgs/action/FollowPath.action b/urc_msgs/action/FollowPath.action index 129ff36a..391dd54d 100644 --- a/urc_msgs/action/FollowPath.action +++ b/urc_msgs/action/FollowPath.action @@ -4,6 +4,7 @@ nav_msgs/Path path # response definition uint16 SUCCESS=0 uint16 FAILURE=1 +uint16 OBSTACLE_DETECTED=2 std_msgs/Empty result uint16 error_code diff --git a/urc_navigation/path_planning/src/astar.cpp b/urc_navigation/path_planning/src/astar.cpp index 17e54b68..cf8add1d 100644 --- a/urc_navigation/path_planning/src/astar.cpp +++ b/urc_navigation/path_planning/src/astar.cpp @@ -7,8 +7,8 @@ AStar::AStar() {} Coordinate AStar::getCoordinateByPose(const geometry_msgs::msg::Pose & pose) { - int x = pose.position.x / costmap_.info.resolution; - int y = pose.position.y / costmap_.info.resolution; + int x = (pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution; + int y = (pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution; return {x, y}; } @@ -96,7 +96,14 @@ double AStar::cost(const AStarNode * from, const AStarNode * to) { Coordinate to_coord = getCoordinateByPose(to->getPose()); int costmap_index = to_coord.y * costmap_.info.width + to_coord.x; - double cell_cost = costmap_.data[costmap_index]; + + + double cell_cost = 1.0; + + // Check cell cost if index is within the costmap + if (costmap_index >= 0 && costmap_index < costmap_.data.size()) { + cell_cost = costmap_.data[costmap_index]; + } double distance = std::sqrt(std::pow(to->x - from->x, 2) + std::pow(to->y - from->y, 2)); return distance * cell_cost; diff --git a/urc_navigation/path_planning/src/planner_server.cpp b/urc_navigation/path_planning/src/planner_server.cpp index 20cc88e3..5927b776 100644 --- a/urc_navigation/path_planning/src/planner_server.cpp +++ b/urc_navigation/path_planning/src/planner_server.cpp @@ -55,7 +55,7 @@ void PlannerServer::generatePlan( for (auto & node : path) { geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "odom"; + pose.header.frame_id = "map"; pose.header.stamp = get_clock()->now(); pose.pose = node.getPose(); @@ -63,7 +63,7 @@ void PlannerServer::generatePlan( } nav_msgs::msg::Path plan; - plan.header.frame_id = "odom"; + plan.header.frame_id = "map"; plan.header.stamp = get_clock()->now(); plan.poses = poses; diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml index 5b905789..1c06c810 100644 --- a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml +++ b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml @@ -2,7 +2,8 @@ follower_action_server: ros__parameters: lookahead_distance: 0.9 desired_linear_velocity: 0.5 - cmd_vel_topic: "/diff_cont/cmd_vel_unstamped" - odom_topic: "/diff_cont/odom" - map_frame: "odom" + cmd_vel_stamped: true + cmd_vel_topic: "/rover_drivetrain_controller/cmd_vel" + odom_topic: "/rover_drivetrain_controller/odom" + map_frame: "map" goal_tolerance: 0.1 diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 84864f54..98382c10 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -12,6 +12,7 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "urc_msgs/action/follow_path.hpp" +#include namespace follower_action_server { @@ -23,12 +24,16 @@ class FollowerActionServer : public rclcpp::Node ~FollowerActionServer(); private: - geometry_msgs::msg::TransformStamped lookup_map_to_base_link(); + 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); + void publishZeroVelocity(); + rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); @@ -42,8 +47,19 @@ class FollowerActionServer : public rclcpp::Node 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); + + 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_; @@ -53,6 +69,7 @@ class FollowerActionServer : public rclcpp::Node rclcpp::Publisher::SharedPtr marker_pub_; geometry_msgs::msg::PoseStamped current_pose_; + bool stamped_; }; } // 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 c4f05e52..0d1f3433 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -18,13 +18,27 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) declare_parameter("odom_topic", "/odom"); declare_parameter("map_frame", "map"); declare_parameter("goal_tolerance", 0.1); + declare_parameter("cmd_vel_stamped", false); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - cmd_vel_pub_ = create_publisher( - get_parameter( - "cmd_vel_topic").as_string(), 10); + stamped_ = get_parameter("cmd_vel_stamped").as_bool(); + + if (stamped_) { + cmd_vel_stamped_pub_ = + create_publisher( + get_parameter( + "cmd_vel_topic") + .as_string(), + 10); + } else { + cmd_vel_pub_ = create_publisher( + get_parameter( + "cmd_vel_topic") + .as_string(), + 10); + } carrot_pub_ = create_publisher("carrot", 10); marker_pub_ = create_publisher("lookahead_circle", 10); @@ -40,6 +54,12 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) current_pose_ = pose; }); + // Setup the costmap + costmap_subscriber_ = create_subscription( + "/costmap", + rclcpp::SystemDefaultsQoS(), + std::bind(&FollowerActionServer::handleCostmap, this, std::placeholders::_1)); + // Create an action server for the follow_path action follow_path_server_ = rclcpp_action::create_server( this, @@ -51,14 +71,19 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) std::bind(&FollowerActionServer::handle_accepted, this, std::placeholders::_1)); } -geometry_msgs::msg::TransformStamped FollowerActionServer::lookup_map_to_base_link() +void FollowerActionServer::handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + current_costmap_ = *msg; +} + +geometry_msgs::msg::TransformStamped FollowerActionServer::lookup_transform( + std::string target_frame, std::string source_frame) { - std::string map_frame = get_parameter("map_frame").as_string(); geometry_msgs::msg::TransformStamped transform; try { - transform = tf_buffer_->lookupTransform("base_link", map_frame, tf2::TimePointZero); + transform = tf_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(this->get_logger(), "Could not transform path to base_link: %s", ex.what()); + RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what()); } return transform; } @@ -134,6 +159,32 @@ visualization_msgs::msg::Marker FollowerActionServer::create_lookahead_circle( return circle; } +void FollowerActionServer::publishZeroVelocity() +{ + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header.stamp = get_clock()->now(); + cmd_vel.twist.linear.x = 0.0; + cmd_vel.twist.angular.z = 0.0; + + if (stamped_) { + cmd_vel_stamped_pub_->publish(cmd_vel); + } else { + cmd_vel_pub_->publish(cmd_vel.twist); + } +} + +int FollowerActionServer::getCost(const nav_msgs::msg::OccupancyGrid & costmap, double x, double y) +{ + int map_x = (x - costmap.info.origin.position.x) / costmap.info.resolution; + int map_y = (y - costmap.info.origin.position.y) / costmap.info.resolution; + + if (map_x < 0 || map_x >= costmap.info.width || map_y < 0 || map_y >= costmap.info.height) { + return 0; + } + + return costmap.data[map_y * costmap.info.width + map_x]; +} + void FollowerActionServer::execute( const std::shared_ptr> goal_handle) { @@ -153,47 +204,64 @@ void FollowerActionServer::execute( pure_pursuit.setPath(path); - // Create a timer to publish the carrot point - auto timer = create_wall_timer( - std::chrono::milliseconds(100), - [this, &pure_pursuit, &path, &feedback, &goal_handle, ¶ms]() - { - auto output = pure_pursuit.getCommandVelocity(lookup_map_to_base_link()); - cmd_vel_pub_->publish(output.cmd_vel.twist); - - auto circle = - create_lookahead_circle( - current_pose_.pose.position.x, current_pose_.pose.position.y, - params.lookahead_distance, get_parameter("map_frame").as_string()); - marker_pub_->publish(circle); - - // Publish the carrot point - carrot_pub_->publish(output.lookahead_point); + pure_pursuit::PurePursuitOutput output; + rclcpp::Rate rate(10); - // Publish feedback - feedback->distance_to_goal = - geometry_util::dist2D(current_pose_.pose.position, path.poses.back().pose.position); - goal_handle->publish_feedback(feedback); - }); - - // Wait for the goal to be canceled while (rclcpp::ok()) { if (goal_handle->is_canceling()) { goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal has been canceled"); - return; - } - - if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double()) { + break; + } else if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double()) { result->error_code = urc_msgs::action::FollowPath::Result::SUCCESS; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal has been reached!"); - return; + break; + } else if (getCost( + current_costmap_, output.lookahead_point.point.x, + output.lookahead_point.point.y) > 0) + { + result->error_code = urc_msgs::action::FollowPath::Result::OBSTACLE_DETECTED; + goal_handle->abort(result); + RCLCPP_INFO(this->get_logger(), "Obstacle detected!"); + break; + } + + output = + pure_pursuit.getCommandVelocity( + lookup_transform( + "base_link", + get_parameter("map_frame").as_string())); + + auto odom_to_map_ = lookup_transform(get_parameter("map_frame").as_string(), "odom"); + + if (stamped_) { + cmd_vel_stamped_pub_->publish(output.cmd_vel); + } else { + cmd_vel_pub_->publish(output.cmd_vel.twist); } + + geometry_msgs::msg::PoseStamped current_pose_map_frame_; + tf2::doTransform(current_pose_, current_pose_map_frame_, odom_to_map_); + + auto circle = + create_lookahead_circle( + current_pose_map_frame_.pose.position.x, current_pose_map_frame_.pose.position.y, + params.lookahead_distance, get_parameter("map_frame").as_string()); + marker_pub_->publish(circle); + + // Publish the carrot point + carrot_pub_->publish(output.lookahead_point); + + // Publish feedback + feedback->distance_to_goal = + geometry_util::dist2D(current_pose_map_frame_.pose.position, path.poses.back().pose.position); + goal_handle->publish_feedback(feedback); + + rate.sleep(); } - // Cancel the timer - timer->cancel(); + publishZeroVelocity(); } } // namespace follower_node diff --git a/urc_perception/launch/perception.launch.py b/urc_perception/launch/perception.launch.py index cbb95910..b5e71829 100644 --- a/urc_perception/launch/perception.launch.py +++ b/urc_perception/launch/perception.launch.py @@ -1,8 +1,8 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node +from launch_ros.actions import Node, SetRemap from launch_ros.substitutions import FindPackageShare -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, GroupAction from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -69,15 +69,32 @@ def generate_launch_description(): ), ) - realsense = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [FindPackageShare("realsense2_camera"), "launch", "rs_launch.py"] - ) - ] - ), + # /camera/depth/color/points -> /depth_camera/points + realsense = GroupAction( + actions=[ + SetRemap(src="/camera/depth/color/points", dst="/depth_camera/points"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("realsense2_camera"), + "launch", + "rs_launch.py", + ] + ) + ] + ), + launch_arguments={"pointcloud.enable": "True"}.items(), + ), + ] ) + # realsense = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # [PathJoinSubstitution([FindPackageShare( + # "realsense2_camera"), "launch", "rs_launch.py"])] + # ), + # ) return LaunchDescription( [ diff --git a/urc_test/launch/odom_to_map_pose.launch.py b/urc_test/launch/odom_to_map_pose.launch.py new file mode 100644 index 00000000..0bb4e642 --- /dev/null +++ b/urc_test/launch/odom_to_map_pose.launch.py @@ -0,0 +1,22 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = LaunchDescription() + + odom_to_map_pose_node = Node( + package="urc_test", + executable="odom_to_map_pose", + name="odom_to_map_pose", + output="screen", + ) + + # finalize + ld.add_action(odom_to_map_pose_node) + + return ld diff --git a/urc_test/setup.py b/urc_test/setup.py index db0531f5..c7d6cb56 100644 --- a/urc_test/setup.py +++ b/urc_test/setup.py @@ -14,7 +14,9 @@ (os.path.join("share", package_name, "launch"), glob("launch/*.py")), (os.path.join("share", package_name, "config"), glob("config/*.yaml")), ], - package_dir={'': 'src', }, + package_dir={ + "": "src", + }, install_requires=["setuptools"], zip_safe=True, maintainer="nvidia", @@ -26,7 +28,8 @@ "console_scripts": [ "urc_test = urc_test.urc_test:main", "costmap_generator = urc_test.costmap_generator:main", - "imu_rpy = tester.pub_imu_rpy:main", + "imu_rpy = urc_test.pub_imu_rpy:main", + "odom_to_map_pose = urc_test.odom_to_map_pose:main", ], }, ) diff --git a/urc_test/src/urc_test/odom_to_map_pose.py b/urc_test/src/urc_test/odom_to_map_pose.py new file mode 100644 index 00000000..31c10851 --- /dev/null +++ b/urc_test/src/urc_test/odom_to_map_pose.py @@ -0,0 +1,57 @@ +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Odometry + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +import tf2_geometry_msgs + + +class OdomToMapPose(Node): + + def __init__(self): + super().__init__("odom_to_map_pose_node") + + # Declare and acquire `target_frame` parameter + self.target_frame = "map" + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.publisher = self.create_publisher(PoseStamped, "/map/pose", 1) + self.subscription = self.create_subscription( + Odometry, "/rover_drivetrain_controller/odom", self.odom_callback, 1 + ) + + def odom_callback(self, msg): + from_frame_rel = "odom" + to_frame_rel = self.target_frame + + try: + t = self.tf_buffer.lookup_transform( + to_frame_rel, from_frame_rel, rclpy.time.Time() + ) + except TransformException as ex: + self.get_logger().info( + f"Could not transform {to_frame_rel} to {from_frame_rel}: {ex}" + ) + return + + pose_msg = PoseStamped() + pose_msg.header = msg.header + + # transform pose in msg to target_frame + pose_msg.pose = tf2_geometry_msgs.do_transform_pose(msg.pose.pose, t) + + self.publisher.publish(pose_msg) + + +def main(): + rclpy.init() + node = OdomToMapPose() + rclpy.spin(node) + + rclpy.shutdown()