diff --git a/Makefile b/Makefile index 7cc5ef0..164dfbd 100644 --- a/Makefile +++ b/Makefile @@ -28,6 +28,9 @@ run: run-realsense: ros2 launch realsense2_camera rs_launch.py initial_reset:=true pointcloud.enable:=true publish_tf:=false enable_infra1:=true enable_depth:=true enable_gyro:=true enable_accel:=true unite_imu_method:=1 +run-foxglove: + ros2 launch foxglove_bridge foxglove_bridge_launch.xml + rsp: ros2 launch src/openmower/launch/rsp.launch.py diff --git a/src/openmower/CMakeLists.txt b/src/openmower/CMakeLists.txt index f5aefaf..ec3015b 100644 --- a/src/openmower/CMakeLists.txt +++ b/src/openmower/CMakeLists.txt @@ -1,6 +1,11 @@ cmake_minimum_required(VERSION 3.8) project(openmower) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -13,6 +18,23 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(robot_localization REQUIRED) + +add_executable(openmower_datum_publisher_node + src/datum_publisher/node.cpp + src/datum_publisher/node.hpp + src/datum_publisher/node_main.cpp +) + +ament_target_dependencies(openmower_datum_publisher_node + rclcpp + std_msgs + sensor_msgs + geographic_msgs + robot_localization +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -26,10 +48,14 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -INSTALL(DIRECTORY config launch description worlds maps +INSTALL( + DIRECTORY config launch description worlds maps DESTINATION share/${PROJECT_NAME} ) -# ament_target_dependencies(rclcpp std_msgs) +# Installation and export info +install(TARGETS openmower_datum_publisher_node + DESTINATION lib/${PROJECT_NAME} +) ament_package() diff --git a/src/openmower/config/nav2_params.yaml b/src/openmower/config/nav2_params.yaml index 322e401..ea49920 100644 --- a/src/openmower/config/nav2_params.yaml +++ b/src/openmower/config/nav2_params.yaml @@ -1,52 +1,52 @@ -amcl: - ros__parameters: - use_sim_time: True - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - set_initial_pose: true - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_topic: /diff_drive_base_controller/odom - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "nav2_amcl::DifferentialMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: /camera/depth/color/points +#amcl: +# ros__parameters: +# use_sim_time: True +# alpha1: 0.2 +# alpha2: 0.2 +# alpha3: 0.2 +# alpha4: 0.2 +# alpha5: 0.2 +# base_frame_id: "base_footprint" +# beam_skip_distance: 0.5 +# beam_skip_error_threshold: 0.9 +# beam_skip_threshold: 0.3 +# do_beamskip: false +# global_frame_id: "map" +# lambda_short: 0.1 +# laser_likelihood_max_dist: 2.0 +# laser_max_range: 100.0 +# laser_min_range: -1.0 +# laser_model_type: "likelihood_field" +# set_initial_pose: true +# max_beams: 60 +# max_particles: 2000 +# min_particles: 500 +# odom_topic: /diff_drive_base_controller/odom +# odom_frame_id: "odom" +# pf_err: 0.05 +# pf_z: 0.99 +# recovery_alpha_fast: 0.0 +# recovery_alpha_slow: 0.0 +# resample_interval: 1 +# robot_model_type: "nav2_amcl::DifferentialMotionModel" +# save_pose_rate: 0.5 +# sigma_hit: 0.2 +# tf_broadcast: true +# transform_tolerance: 1.0 +# update_min_a: 0.2 +# update_min_d: 0.25 +# z_hit: 0.5 +# z_max: 0.05 +# z_rand: 0.5 +# z_short: 0.05 +# scan_topic: /camera/depth/color/points bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /diff_drive_base_controller/odom + odom_topic: /odometry/filtered/map bt_loop_duration: 10 default_server_timeout: 20 navigators: ["navigate_to_pose", "navigate_through_poses"] @@ -197,28 +197,29 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - robot_radius: 0.22 +# robot_radius: 0.5 + footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]" plugins: - - voxel_layer +# - voxel_layer - static_layer - inflation_layer inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /camera/depth/color/points - data_type: "PointCloud2" +# voxel_layer: +# plugin: "nav2_costmap_2d::VoxelLayer" +# enabled: True +# publish_voxel_map: True +# origin_z: 0.0 +# z_resolution: 0.05 +# z_voxels: 16 +# max_obstacle_height: 2.0 +# mark_threshold: 0 +# observation_sources: pointcloud +# pointcloud: +# topic: /camera/depth/color/points +# data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -232,20 +233,20 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - robot_radius: 0.22 + robot_radius: 0.5 resolution: 0.05 track_unknown_space: true plugins: - static_layer - - obstacle_layer +# - obstacle_layer - inflation_layer - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: pointcloud - pointcloud: - topic: /camera/depth/color/points - data_type: "PointCloud2" +# obstacle_layer: +# plugin: "nav2_costmap_2d::ObstacleLayer" +# enabled: True +# observation_sources: pointcloud +# pointcloud: +# topic: /camera/depth/color/points +# data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True diff --git a/src/openmower/config/robot_localization.yaml b/src/openmower/config/robot_localization.yaml index 16f951d..56a0647 100644 --- a/src/openmower/config/robot_localization.yaml +++ b/src/openmower/config/robot_localization.yaml @@ -120,6 +120,19 @@ ekf_se_map: odom1_differential: false odom1_relative: false + # motion heading from GPS + # yaw only + odom2: gps/odom + odom2_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + odom2_queue_size: 10 + odom2_nodelay: true + odom2_differential: false + odom2_relative: false + imu0: imu/data imu0_config: [false, false, false, false, false, true, @@ -174,4 +187,3 @@ navsat_transform_node: publish_filtered_gps: true use_odometry_yaw: true wait_for_datum: true - datum: [52.4955621, 21.3852107, 0.0] # todo: remove my coordinates \ No newline at end of file diff --git a/src/openmower/launch/gps.launch.py b/src/openmower/launch/gps.launch.py index 3103c47..8ff81d3 100644 --- a/src/openmower/launch/gps.launch.py +++ b/src/openmower/launch/gps.launch.py @@ -15,7 +15,15 @@ def generate_launch_description(): name='ntrip_client_node', package='ntrip_client', executable='ntrip_ros.py', - parameters=[os.path.join(get_package_share_directory("openmower"), 'config', 'gps.yaml')], + parameters=[{ + 'host': os.getenv('OM_NTRIP_HOSTNAME'), + 'port': int(os.getenv('OM_NTRIP_PORT')), + 'mountpoint': os.getenv('OM_NTRIP_ENDPOINT'), + 'authenticate': True, + 'username': os.getenv('OM_NTRIP_USER'), + 'password': os.getenv('OM_NTRIP_PASSWORD'), + 'rtcm_message_package': 'rtcm_msgs', + }], ), Node( diff --git a/src/openmower/launch/localization.launch.py b/src/openmower/launch/localization.launch.py index 9cd05d5..0aaab28 100644 --- a/src/openmower/launch/localization.launch.py +++ b/src/openmower/launch/localization.launch.py @@ -129,4 +129,15 @@ def generate_launch_description(): ('/odometry/filtered', '/odometry/filtered/map'), ], ), + + Node( + package='openmower', + executable='openmower_datum_publisher_node', + output='screen', + parameters=[{ + 'datum.latitude': float(os.getenv('OM_DATUM_LAT')), + 'datum.longitude': float(os.getenv('OM_DATUM_LONG')), + 'datum.publish_as_fix': True, + }], + ) ]) diff --git a/src/openmower/src/datum_publisher/README.md b/src/openmower/src/datum_publisher/README.md new file mode 100644 index 0000000..888e4d2 --- /dev/null +++ b/src/openmower/src/datum_publisher/README.md @@ -0,0 +1,17 @@ +# openmower_datum_publisher_node + +## overview + +This node has two responsibilities: + +- call [robot_localization](http://docs.ros.org/en/noetic/api/robot_localization/html/index.html)'s `/datum` service to set the datum of the robot + this is done, so there is no need to fill this info in robot_localization config files +- publish the datum as a [sensor_msgs/NavSatFix](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html) message on the topic `/datum/fix` + mostly to use with Foxglove Studio to see the datum on the map component + +## parameters + +- `datum.latitude` (double, required): latitude of the datum +- `datum.longitude` (double, required): longitude of the datum +- `datum.publish_as_fix` (bool, default: false): publish the datum as a [sensor_msgs/NavSatFix](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html) message on the topic `/datum/fix` +- `datum.fix_topic` (string, default: "/datum/fix"): topic to publish the datum as a [sensor_msgs/NavSatFix](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html) message on \ No newline at end of file diff --git a/src/openmower/src/datum_publisher/node.cpp b/src/openmower/src/datum_publisher/node.cpp new file mode 100644 index 0000000..4517c21 --- /dev/null +++ b/src/openmower/src/datum_publisher/node.cpp @@ -0,0 +1,77 @@ +#include "node.hpp" + +#include + +namespace OpenMower { + namespace DatumPublisher { + Node::Node(const rclcpp::NodeOptions &options) : rclcpp::Node("datum_publisher", options) { + datum_latitude_ = this->declare_parameter("datum.latitude", rclcpp::PARAMETER_DOUBLE); + datum_longitude_ = this->declare_parameter("datum.longitude", rclcpp::PARAMETER_DOUBLE); + + try { + RCLCPP_INFO(this->get_logger(), "Datum latitude: %f", datum_latitude_.get()); + RCLCPP_INFO(this->get_logger(), "Datum longitude: %f", datum_longitude_.get()); + } catch (const rclcpp::ParameterTypeException &e) { + RCLCPP_ERROR(this->get_logger(), "Please provide both datum.latitude and datum.longitude parameters"); + rclcpp::shutdown(); + return; + } + + setDatum(); + + if (!this->declare_parameter("datum.publish_as_fix", false)) { + rclcpp::shutdown(); + return; + } + + std::string topic_name = this->declare_parameter("datum.fix_topic", "datum/fix"); + + navsat_fix_publisher_ = this->create_publisher(topic_name, 10); + publishNavSatFix(); + // create timer and publish navsat fix every 10 seconds due to latching is not implemented well in ROS2 + auto timer = this->create_wall_timer(std::chrono::seconds(10), [this]() { + publishNavSatFix(); + }); + } + + void Node::setDatum() { + rclcpp::Client::SharedPtr setDatumClient = + this->create_client("/datum"); + + while (!setDatumClient->wait_for_service(std::chrono::seconds(2))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the set_datum service. Exiting."); + return; + } + + RCLCPP_INFO(this->get_logger(), "set_datum service not available, waiting again..."); + } + + auto request = robot_localization::srv::SetDatum::Request(); + request.geo_pose.position.latitude = datum_latitude_.get(); + request.geo_pose.position.longitude = datum_longitude_.get(); + + auto result = setDatumClient->async_send_request(std::make_shared(request)); + + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "Failed to set datum"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Datum set successfully"); + } + + void Node::publishNavSatFix() { + sensor_msgs::msg::NavSatFix msg; + msg.latitude = datum_latitude_.get(); + msg.longitude = datum_longitude_.get(); + msg.header.stamp = this->now(); + msg.header.frame_id = "map"; + + navsat_fix_publisher_->publish(msg); + + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 10000, "Published NavSatFix for datum"); + } + } // OpenMower +} // DatumPublisher \ No newline at end of file diff --git a/src/openmower/src/datum_publisher/node.hpp b/src/openmower/src/datum_publisher/node.hpp new file mode 100644 index 0000000..b1dc669 --- /dev/null +++ b/src/openmower/src/datum_publisher/node.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +namespace OpenMower { + namespace DatumPublisher { + + class Node final : public rclcpp::Node { + public: + explicit Node(const rclcpp::NodeOptions &options); + private: + rclcpp::ParameterValue datum_latitude_; + rclcpp::ParameterValue datum_longitude_; + + rclcpp::Publisher::SharedPtr navsat_fix_publisher_; + + void setDatum(); + void publishNavSatFix(); + }; + + } // OpenMower +} // DatumPublisher + diff --git a/src/openmower/src/datum_publisher/node_main.cpp b/src/openmower/src/datum_publisher/node_main.cpp new file mode 100644 index 0000000..e3e2651 --- /dev/null +++ b/src/openmower/src/datum_publisher/node_main.cpp @@ -0,0 +1,18 @@ +#include + +#include + +#include "node.hpp" + +int main(int argc, char **argv) { + // Force flush of the stdout buffer. + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file