From 8c17a95d85d556137a49ec3356278d1d25980e8c Mon Sep 17 00:00:00 2001 From: 1000lai Date: Fri, 6 Feb 2026 22:57:02 +0800 Subject: [PATCH 01/22] Fix: double time = seconds + nanoseconds -> nanoseconds --- devel/core_algorithm/fusion/src/fusion_node.cpp | 8 ++++---- .../local/wheel_inertial_odometry/src/wio_node.cpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index 9764ede..e0dbe22 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -130,7 +130,7 @@ void FusionNode::initCallback(const geometry_msgs::msg::PoseWithCovariance & ini void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ rclcpp::Time stamp = local_msg.header.stamp; - this->prev_pred_time = stamp.seconds()+stamp.nanoseconds()*1e-9; + this->prev_pred_time = stamp.nanoseconds()*1e-9; Eigen::Vector3d local_pose; double local_yaw = utils::qua2yaw(local_msg.pose.pose.orientation); @@ -177,7 +177,7 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam if(if_align){ rclcpp::Clock clock; rclcpp::Time stamp = clock.now(); - double corr_time = stamp.seconds()+stamp.nanoseconds()*1e-9; + double corr_time = stamp.nanoseconds()*1e-9; double dt_align = corr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) @@ -232,7 +232,7 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ if(if_align){ rclcpp::Clock clock; rclcpp::Time stamp = clock.now(); - double corr_time = stamp.seconds()+stamp.nanoseconds()*1e-9; + double corr_time = stamp.nanoseconds()*1e-9; double dt_align = corr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) @@ -287,7 +287,7 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ if(if_align){ rclcpp::Clock clock; rclcpp::Time stamp = clock.now(); - double corr_time = stamp.seconds()+stamp.nanoseconds()*1e-9; + double corr_time = stamp.nanoseconds()*1e-9; double dt_align = corr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp index 592640c..35091e6 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp +++ b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp @@ -77,7 +77,7 @@ void WioNode::wheelCallback(const geometry_msgs::msg::Twist & wheel_msg) { rclcpp::Clock clock; rclcpp::Time stamp = clock.now(); - double odom_time = stamp.seconds() + stamp.nanoseconds() * 1e-9; + double odom_time = stamp.nanoseconds() * 1e-9; if (this->first_odom_cb) { @@ -115,7 +115,7 @@ void WioNode::wheelCallback(const geometry_msgs::msg::Twist & wheel_msg) void WioNode::imuCallback(const sensor_msgs::msg::Imu & imu_msg) { rclcpp::Time stamp = imu_msg.header.stamp; - double time = stamp.seconds()+stamp.nanoseconds()*1e-9; + double time = stamp.nanoseconds()*1e-9; if(this->first_imu_cb){ this->prev_imu_time = time; @@ -130,7 +130,7 @@ void WioNode::imuCallback(const sensor_msgs::msg::Imu & imu_msg) this->local_twist(2) = imu_msg.angular_velocity.z; this->local_pose(2) = - utils::angleNormalizing(this->local_pose(2)+0.5*this->local_twist(2)*dt); + utils::angleNormalizing(this->local_pose(2)+this->local_twist(2)*dt); Eigen::Matrix3d R; R << cos(this->local_pose(2)), -sin(this->local_pose(2)), 0, From 7cad5f92f7c8c7d2b1af6db17cdeb4f5af9d22ab Mon Sep 17 00:00:00 2001 From: 1000lai Date: Fri, 6 Feb 2026 23:03:28 +0800 Subject: [PATCH 02/22] Fix: prevent repeated wheel delta integration in imu callback --- .../wheel_inertial_odometry/wio_node.hpp | 2 -- .../wheel_inertial_odometry/src/wio_node.cpp | 18 +----------------- 2 files changed, 1 insertion(+), 19 deletions(-) diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/include/wheel_inertial_odometry/wio_node.hpp b/devel/core_algorithm/local/wheel_inertial_odometry/include/wheel_inertial_odometry/wio_node.hpp index 44ef358..beb5253 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/include/wheel_inertial_odometry/wio_node.hpp +++ b/devel/core_algorithm/local/wheel_inertial_odometry/include/wheel_inertial_odometry/wio_node.hpp @@ -31,8 +31,6 @@ class WioNode : public rclcpp::Node{ std::string robot_parent_frame; std::string robot_frame; - Eigen::Vector3d prev_wheel_pose; - Eigen::Vector3d d_position; Eigen::Vector3d local_pose; Eigen::Vector3d local_twist; Eigen::Matrix3d local_pose_cov; diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp index 35091e6..4a8535a 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp +++ b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp @@ -79,24 +79,9 @@ void WioNode::wheelCallback(const geometry_msgs::msg::Twist & wheel_msg) rclcpp::Time stamp = clock.now(); double odom_time = stamp.nanoseconds() * 1e-9; - if (this->first_odom_cb) - { - this->prev_wheel_pose << wheel_msg.angular.x, wheel_msg.angular.y; - this->prev_odom_time = odom_time; - this->first_odom_cb = false; - return; - } - double dt = odom_time - this->prev_odom_time; this->prev_odom_time = odom_time; - - this->d_position << wheel_msg.angular.x-this->prev_wheel_pose(0), - wheel_msg.angular.y-this->prev_wheel_pose(1), - 0; - - this->prev_wheel_pose << wheel_msg.angular.x, wheel_msg.angular.y; - this->local_twist(0) = wheel_msg.linear.x; this->local_twist(1) = wheel_msg.linear.y; @@ -126,7 +111,6 @@ void WioNode::imuCallback(const sensor_msgs::msg::Imu & imu_msg) double dt = time-this->prev_imu_time; this->prev_imu_time = time; - this->local_twist(2) = imu_msg.angular_velocity.z; this->local_pose(2) = @@ -137,7 +121,7 @@ void WioNode::imuCallback(const sensor_msgs::msg::Imu & imu_msg) sin(this->local_pose(2)), cos(this->local_pose(2)), 0, 0, 0, 0; - this->local_pose += R*this->d_position; + this->local_pose += R*this->local_twist*dt; // publish odometry pubLocalPose(stamp); From 4be3368bb950d271d1a49045a7c0867bc61020f1 Mon Sep 17 00:00:00 2001 From: 1000lai Date: Sat, 7 Feb 2026 00:38:48 +0800 Subject: [PATCH 03/22] Fix: delete unused things --- .../local/wheel_inertial_odometry/src/wio_node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp index 4a8535a..732fd9c 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp +++ b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp @@ -15,8 +15,6 @@ WioNode::WioNode() : Node("wio_node"){ for(int i=0; i<3; i++) this->pose_cov_min[i] = 0; this->first_odom_cb = true; this->prev_odom_time = 0.0; - this->prev_wheel_pose.setZero(); - this->d_position.setZero(); this->local_pose.setZero(); this->local_twist.setZero(); this->local_pose_cov.setIdentity(); From 3101dab69b1a4945f383454853d01761b4928661 Mon Sep 17 00:00:00 2001 From: 1000lai Date: Sat, 7 Feb 2026 17:26:27 +0800 Subject: [PATCH 04/22] =?UTF-8?q?use=20map=E2=86=92odom=20+=20local=5Fpose?= =?UTF-8?q?=20for=20ekf=20prediction?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fusion/include/fusion/fusion_node.hpp | 2 ++ devel/core_algorithm/fusion/src/fusion_node.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp index 360f530..41fbce9 100644 --- a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp +++ b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp @@ -49,10 +49,12 @@ class FusionNode : public rclcpp::Node{ Eigen::Vector3d local_twist; Eigen::Vector3d init_state; + Eigen::Vector3d mapodom_state; Eigen::Matrix3d local_cov; Eigen::Matrix3d cbcam_cov; Eigen::Matrix3d obcam_cov; Eigen::Matrix3d init_rot; + Eigen::Matrix3d mapodom_rot; bool if_align; diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index e0dbe22..e36301f 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -146,7 +146,17 @@ void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ 0, local_msg.pose.covariance[7], 0, 0, 0, local_msg.pose.covariance[35]; - Eigen::Vector3d predicted_pose = this->init_state + this->init_rot*local_pose; + geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform("map", "odom", stamp); + + this->mapodom_state(0) = tf.transform.translation.x; + this->mapodom_state(1) = tf.transform.translation.y; + this->mapodom_state(2) = utils::qua2yaw(tf.transform.rotation); + + this->mapodom_rot << cos(this->mapodom_state(2)), -sin(this->mapodom_state(2)), 0, + sin(this->mapodom_state(2)), cos(this->mapodom_state(2)), 0, + 0, 0, 1; + + Eigen::Vector3d predicted_pose = this->mapodom_state + this->mapodom_rot*local_pose; ekf.Xprediction(predicted_pose, this->local_cov); Eigen::Vector3d robot_pose = ekf.getPose(); From d80743996884c9be2553101ca2435d37eccd82a8 Mon Sep 17 00:00:00 2001 From: robert Date: Thu, 12 Feb 2026 01:43:37 +0800 Subject: [PATCH 05/22] Modify: uncommited changes on bravo bot - repname parameter ```side``` in ```lidar_member_function.py``` to ```mode``` - change ```self.side``` type to string - change firmware port name - specify topics in ros1_bridge --- .../scripts/lidar_member_function.py | 20 ++-- devel/monitor/src/data_rate_test.cpp | 95 +++++++++++++++++++ docker/.env | 1 + docker/config/bridge.yaml | 9 ++ docker/docker-compose.yaml | 22 +++-- .../firmware_comm/launch/firmware_comm.launch | 2 +- 6 files changed, 130 insertions(+), 19 deletions(-) create mode 100644 devel/monitor/src/data_rate_test.cpp create mode 100644 docker/config/bridge.yaml diff --git a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py index 1934ae3..ea29390 100644 --- a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py +++ b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py @@ -19,7 +19,7 @@ def __init__(self): super().__init__('lidar_localization_node') # Declare parameters - self.declare_parameter('side', 0) + self.declare_parameter('mode', 'yellow') self.declare_parameter('debug_mode', False) self.declare_parameter('use_two_beacons', False) self.declare_parameter('visualize_candidate', True) @@ -32,7 +32,7 @@ def __init__(self): self.declare_parameter('consistency_threshold_two', 0.99) # Get parameters - self.side = self.get_parameter('side').get_parameter_value().integer_value + self.side = self.get_parameter('mode').get_parameter_value().string_value self.debug_mode = self.get_parameter('debug_mode').get_parameter_value().bool_value self.p_use_two_beacons = self.get_parameter('use_two_beacons').get_parameter_value().bool_value self.visualize_true = self.get_parameter('visualize_candidate').get_parameter_value().bool_value @@ -45,13 +45,13 @@ def __init__(self): self.consistency_threshold_two = self.get_parameter('consistency_threshold_two').get_parameter_value().double_value # Set the landmarks map based on the side - if self.side == 0: + if self.side == 'yellow': self.landmarks_map = [ np.array([-0.083, 0.041]), np.array([-0.083, 1.959]), np.array([3.083, 1.0]) ] - elif self.side == 1: + elif self.side == 'blue': self.landmarks_map = [ np.array([3.083, 0.041]), np.array([3.083, 1.959]), @@ -215,14 +215,14 @@ def set_lidar_side_callback(self, msg): side = msg.data.lower() if side in ['0', '1', 'yellow', 'blue']: if side == '0' or side == 'yellow': - self.side = 0 + self.side = 'yellow' self.landmarks_map = [ np.array([-0.083, 0.041]), np.array([-0.083, 1.959]), np.array([3.083, 1.0]) ] elif side == '1' or side == 'blue': - self.side = 1 + self.side = 'blue' self.landmarks_map = [ np.array([3.083, 0.041]), np.array([3.083, 1.959]), @@ -478,11 +478,11 @@ def get_set_two(self, two_index): # geometry consistency # 1. check the cross product: 'robot to beacon 0' cross 'robot to beacon 1' cross = np.cross(set['beacons'][0], set['beacons'][1]) # TODO: check if beacon 0 and 1 will be in the order of beacon a, b and c - if self.side == 0: # yellow is clockwise(negative) + if self.side == 'yellow': # yellow is clockwise(negative) if cross > 0: # self.get_logger().debug("cross product is positive") continue - elif self.side == 1: # blue is counter-clockwise(positive) + elif self.side == 'blue': # blue is counter-clockwise(positive) if cross < 0: # self.get_logger().debug("cross product is negative") continue @@ -676,10 +676,10 @@ def get_geometry_consistency(self, beacons): # if the index is not found in map, it is probably on the lower triangle of the matrix # check the landmark sequence is correct, clockwise for yellow, counter-clockwise for blue - if self.side == 0: + if self.side == 'yellow': if np.cross(beacons[1] - beacons[0], beacons[2] - beacons[0]) > 0: consistency = 0 - elif self.side == 1: + elif self.side == 'blue': if np.cross(beacons[1] - beacons[0], beacons[2] - beacons[0]) < 0: consistency = 0 diff --git a/devel/monitor/src/data_rate_test.cpp b/devel/monitor/src/data_rate_test.cpp new file mode 100644 index 0000000..9d73eb1 --- /dev/null +++ b/devel/monitor/src/data_rate_test.cpp @@ -0,0 +1,95 @@ +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + +class RateTest : public rclcpp::Node{ + public: + RateTest(): Node("data_rate_test"){ + this->wheel_sub = this->create_subscription( + "wheel_pose", 1, std::bind(&RateTest::wheelCallback, this, std::placeholders::_1)); + + this->imu_sub = this->create_subscription( + "imu/data_cov", 1, std::bind(&RateTest::imuCallback, this, std::placeholders::_1)); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.best_effort(); + qos.durability_volatile(); + + this->scan_sub = this->create_subscription( + "scan", qos, std::bind(&RateTest::scanCallback, this, std::placeholders::_1)); + + this->global_sub = this->create_subscription( + "lidar_pose", 1, std::bind(&RateTest::globalCallback, this, std::placeholders::_1)); + + + this->wheel_pub = this->create_publisher("slow_wheel_pose", 10); + this->imu_pub = this->create_publisher("slow_imu/data_cov", 10); + this->scan_pub = this->create_publisher("slow_scan", 10); + this->global_pub = this->create_publisher("slow_lidar_pose", 10); + + + this->declare_parameter("rate_fac", 1.0); + this->rate_fac = this->get_parameter("rate_fac").as_double(); + } + + void wheelCallback(const geometry_msgs::msg::Twist & wheel_msg){ + this->wheel_count++; + if(this->wheel_count >= 1.0/this->rate_fac){ + this->wheel_pub->publish(wheel_msg); + this->wheel_count = 0; + } + } + + void imuCallback(const sensor_msgs::msg::Imu & imu_msg){ + this->imu_count++; + if(this->imu_count >= 1.0/this->rate_fac){ + this->imu_pub->publish(imu_msg); + this->imu_count = 0; + } + } + + void scanCallback(const sensor_msgs::msg::LaserScan & scan_msg){ + this->scan_count++; + if(this->scan_count >= 1.0/this->rate_fac){ + this->scan_pub->publish(scan_msg); + this->scan_count = 0; + } + } + + void globalCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & global_msg){ + this->global_count++; + if(this->global_count >= 1.0/this->rate_fac){ + this->global_pub->publish(global_msg); + this->global_count = 0; + } + } + + private: + rclcpp::Subscription::SharedPtr wheel_sub; + rclcpp::Subscription::SharedPtr imu_sub; + rclcpp::Subscription::SharedPtr scan_sub; + rclcpp::Subscription::SharedPtr global_sub; + + rclcpp::Publisher::SharedPtr wheel_pub; + rclcpp::Publisher::SharedPtr imu_pub; + rclcpp::Publisher::SharedPtr scan_pub; + rclcpp::Publisher::SharedPtr global_pub; + + double rate_fac = 0.0; + + int wheel_count = 0; + int imu_count = 0; + int scan_count = 0; + int global_count = 0; +}; + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/docker/.env b/docker/.env index d7ae865..86d5e20 100644 --- a/docker/.env +++ b/docker/.env @@ -8,6 +8,7 @@ ROS_DOMAIN_ID=13 ## Release version COMPOSE_PROFILES = communication, onboard-running USER_UID=1004 +MODE=yellow ## Simply on local machine # COMPOSE_PROFILES=local-developing diff --git a/docker/config/bridge.yaml b/docker/config/bridge.yaml new file mode 100644 index 0000000..e0c0704 --- /dev/null +++ b/docker/config/bridge.yaml @@ -0,0 +1,9 @@ +topics: + - + topic: /wheel_pose + type: geometry_msgs/msg/Twist + queue_size: 10 + - + topic: /cmd_vel + type: geometry_msgs/msg/Twist + queue_size: 10 \ No newline at end of file diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 339f5ca..86e9c3e 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -57,7 +57,10 @@ services: - *ros2-vars command: /bin/bash -c " source /opt/ros/humble/setup.bash && - colcon build" + cd /home/user/localization-ws && + colcon build && + source install/setup.bash && + ros2 launch localization_bringup localization.launch.py mode:=${MODE}" # containers for comunicating with firmware ros-core: @@ -79,8 +82,7 @@ services: volumes: - /etc/timezone:/etc/timezone:ro - /etc/localtime:/etc/localtime:ro - - /dev/dri:/dev/dri - - /dev/shm:/dev/shm + - /dev:/dev command: bash -c "source /opt/ros/noetic/setup.bash && roscore" @@ -97,6 +99,7 @@ services: stdin_open: true tty: true working_dir: / + restart: on-failure depends_on: ros-core: # The healthcheck is required for ros1-bridge to wait until roscore is ready. @@ -109,12 +112,11 @@ services: volumes: - /etc/timezone:/etc/timezone:ro - /etc/localtime:/etc/localtime:ro - - /dev/dri:/dev/dri - - /dev/shm:/dev/shm + - /dev:/dev command: bash -c "source /ros2_humble/install/setup.bash && source /ros2_humble/install/ros1_bridge/share/ros1_bridge/local_setup.bash && export ROS_MASTER_URI=http://localhost:11311 - && ros2 run ros1_bridge dynamic_bridge 2>/dev/null" + && ros2 run ros1_bridge parameter_bridge 2>/dev/null" ros1: profiles: ['communication'] @@ -124,15 +126,19 @@ services: tty: true network_mode: host privileged: true + depends_on: + ros-core: + condition: service_healthy volumes: - /etc/timezone:/etc/timezone:ro - /etc/localtime:/etc/localtime:ro - - /dev/dri:/dev/dri - - /dev/shm:/dev/shm + - /dev:/dev - ../docker/sensor_dep_ros1:/home/user/ros1-ws/src + - ../docker/config:/home/config stop_grace_period: 1s command: /bin/bash -c " source /opt/ros/noetic/setup.bash && + rosparam load /home/config/bridge.yaml && cd /home/user/ros1-ws && catkin_make && source devel/setup.bash && diff --git a/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch b/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch index 3dc7378..3cd3a7f 100644 --- a/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch +++ b/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch @@ -1,7 +1,7 @@ - + require: From 92929fab01b30cb54f8088699acbc49e51be74e3 Mon Sep 17 00:00:00 2001 From: robert Date: Thu, 12 Feb 2026 02:07:36 +0800 Subject: [PATCH 06/22] Modify: path name --- devel/core_algorithm/fusion/launch/fusion.launch.xml | 2 +- devel/localization_bringup/launch/bag_test.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/devel/core_algorithm/fusion/launch/fusion.launch.xml b/devel/core_algorithm/fusion/launch/fusion.launch.xml index 2a9f491..6f93f56 100644 --- a/devel/core_algorithm/fusion/launch/fusion.launch.xml +++ b/devel/core_algorithm/fusion/launch/fusion.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/devel/localization_bringup/launch/bag_test.launch.py b/devel/localization_bringup/launch/bag_test.launch.py index aa7ab01..22d4481 100644 --- a/devel/localization_bringup/launch/bag_test.launch.py +++ b/devel/localization_bringup/launch/bag_test.launch.py @@ -5,7 +5,7 @@ import yaml def generate_launch_description(): - config_path='/home/user/localization-ws/src/core_algorithm/localization_bringup/param/bag_test.yaml' + config_path='/home/user/localization-ws/src/devel/localization_bringup/param/bag_test.yaml' with open(config_path, 'r') as f: cfg = yaml.safe_load(f) From 2fea89406ea31845339f9022cc3f68dcd3a1d89c Mon Sep 17 00:00:00 2001 From: robert Date: Tue, 24 Feb 2026 19:23:48 +0800 Subject: [PATCH 07/22] Modify: queue size, topic names, param setting, mount building tools - decrease queue size of ros1 bridge to 1 - rename topic names when bag testing - get param ```mode``` in launch - mount folders to contain things for colcon build --- .../lidar_localization/param/lidar_localization.yml | 2 +- .../scripts/lidar_member_function.py | 4 ++-- devel/localization_bringup/launch/bag_test.launch.py | 2 +- .../launch/localization.launch.py | 3 ++- devel/localization_bringup/param/bag_test.yaml | 12 +++++++++--- docker/config/bridge.yaml | 4 ++-- docker/docker-compose.yaml | 3 +++ 7 files changed, 20 insertions(+), 10 deletions(-) diff --git a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml index eeeef19..e6bb8fa 100644 --- a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml +++ b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml @@ -3,6 +3,6 @@ lidar_localization: side: 0 debug_mode: false visualize_candidate: true - likelihood_threshold: 0.8 + likelihood_threshold: 0.85 consistency_threshold: 0.92 lidar_multiplier: 0.987 \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py index ea29390..37019fa 100644 --- a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py +++ b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py @@ -23,8 +23,8 @@ def __init__(self): self.declare_parameter('debug_mode', False) self.declare_parameter('use_two_beacons', False) self.declare_parameter('visualize_candidate', True) - self.declare_parameter('likelihood_threshold', 0.8) - self.declare_parameter('consistency_threshold', 0.92) + self.declare_parameter('likelihood_threshold', 0.87) + self.declare_parameter('consistency_threshold', 0.93) self.declare_parameter('robot_frame_id', 'base_footprint') self.declare_parameter('robot_parent_frame_id', 'map') self.declare_parameter('lidar_multiplier', 0.987) diff --git a/devel/localization_bringup/launch/bag_test.launch.py b/devel/localization_bringup/launch/bag_test.launch.py index 22d4481..0aeb489 100644 --- a/devel/localization_bringup/launch/bag_test.launch.py +++ b/devel/localization_bringup/launch/bag_test.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): output='screen' ), GroupAction([ - PushRosNamespace('eurobot2025'), + PushRosNamespace('pose_info'), ExecuteProcess( cmd=[ 'ros2', 'bag', 'play', diff --git a/devel/localization_bringup/launch/localization.launch.py b/devel/localization_bringup/launch/localization.launch.py index 8a942ed..1230056 100644 --- a/devel/localization_bringup/launch/localization.launch.py +++ b/devel/localization_bringup/launch/localization.launch.py @@ -61,7 +61,8 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', 'info'], output='screen', parameters=[ - '/home/user/localization-ws/src/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml' + '/home/user/localization-ws/src/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml', + {'mode': mode} ] ) diff --git a/devel/localization_bringup/param/bag_test.yaml b/devel/localization_bringup/param/bag_test.yaml index 19fe214..08acfe6 100644 --- a/devel/localization_bringup/param/bag_test.yaml +++ b/devel/localization_bringup/param/bag_test.yaml @@ -3,7 +3,10 @@ bag: path: src/core_algorithm/localization_bringup/bag/ name: test1 record_topics: - - /odoo_googoogoo + - /clock + - /tf + - /tf_static + - /wheel_pose - /scan - /raw_obstacles - /imu/data_cov @@ -14,12 +17,15 @@ bag: - /candidates play_topics: raw: - - /odoo_googoogoo + - /clock + - /tf + - /tf_static + - /wheel_pose - /scan - /raw_obstacles - /imu/data_cov result: - - /local_filter + - /local_pose - /lidar_pose - /final_pose - /beacons_guaguagua diff --git a/docker/config/bridge.yaml b/docker/config/bridge.yaml index e0c0704..e2639ed 100644 --- a/docker/config/bridge.yaml +++ b/docker/config/bridge.yaml @@ -2,8 +2,8 @@ topics: - topic: /wheel_pose type: geometry_msgs/msg/Twist - queue_size: 10 + queue_size: 1 - topic: /cmd_vel type: geometry_msgs/msg/Twist - queue_size: 10 \ No newline at end of file + queue_size: 1 \ No newline at end of file diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 86e9c3e..43ea53f 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -26,6 +26,9 @@ x-common-vars: &ros2-vars - /dev:/dev # Mount workspace - ../devel:/home/user/localization-ws/src/devel + - ../colcon/build:/home/user/localization-ws/build + - ../colcon/install:/home/user/localization-ws/install + - ../colcon/log:/home/user/localization-ws/log services: localization-devel: From bc0ac0e35e6c76bb31cadac1a9a9a7aa2315e2cc Mon Sep 17 00:00:00 2001 From: robert Date: Tue, 24 Feb 2026 22:29:47 +0800 Subject: [PATCH 08/22] Modify: accumulate process noise --- devel/core_algorithm/fusion/src/ekf.cpp | 4 ++-- .../local/wheel_inertial_odometry/launch/local.launch.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/devel/core_algorithm/fusion/src/ekf.cpp b/devel/core_algorithm/fusion/src/ekf.cpp index fa9948a..b361b3e 100644 --- a/devel/core_algorithm/fusion/src/ekf.cpp +++ b/devel/core_algorithm/fusion/src/ekf.cpp @@ -21,7 +21,7 @@ void EKF::Xprediction(Eigen::Vector3d x, Eigen::Matrix3d R){ if(this->robot_state.mu(2) > M_PI || this->robot_state.mu(2) < -M_PI) this->robot_state.mu(2) = angleNormalizing(this->robot_state.mu(2)); - this->robot_state.sigma = R; + this->robot_state.sigma += R; } void EKF::Vprediction(Eigen::Vector3d u, Eigen::Matrix3d R, double dt){ @@ -40,7 +40,7 @@ void EKF::Vprediction(Eigen::Vector3d u, Eigen::Matrix3d R, double dt){ if(this->robot_state.mu(2) > M_PI || this->robot_state.mu(2) < -M_PI) this->robot_state.mu(2) = angleNormalizing(this->robot_state.mu(2)); - this->robot_state.sigma = R; + this->robot_state.sigma += R; } void EKF::correction(Eigen::Vector3d z, Eigen::Matrix3d Q){ diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py index 5450190..1c1cd51 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py +++ b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py @@ -33,9 +33,9 @@ def generate_launch_description(): DeclareLaunchArgument('vx_coeff', default_value='0.3'), DeclareLaunchArgument('vy_coeff', default_value='0.3'), DeclareLaunchArgument('w_coeff', default_value='2.0'), - DeclareLaunchArgument('x_cov_min', default_value='6.25e-4'), - DeclareLaunchArgument('y_cov_min', default_value='6.25e-4'), - DeclareLaunchArgument('theta_cov_min', default_value='0.08'), + DeclareLaunchArgument('x_cov_min', default_value='1.67e-6'), + DeclareLaunchArgument('y_cov_min', default_value='1.67e-6'), + DeclareLaunchArgument('theta_cov_min', default_value='1.07e-4'), ] # IMU firmware and covariance feedback loop node (contain IMU spatial launch) From 843c7bbea9b11c3284d3969b2f7debe0c31c36a8 Mon Sep 17 00:00:00 2001 From: robert Date: Sat, 28 Feb 2026 21:58:57 +0800 Subject: [PATCH 09/22] Modify: map to odom as dynamic transform for semantic correctness --- .../fusion/include/fusion/fusion_node.hpp | 4 ++-- devel/core_algorithm/fusion/src/fusion_node.cpp | 10 +++++----- devel/utils/include/utils/gen_functs.hpp | 3 ++- devel/utils/src/gen_functs.cpp | 4 ++-- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp index 41fbce9..8e67514 100644 --- a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp +++ b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp @@ -10,7 +10,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.h" #include @@ -39,7 +39,7 @@ class FusionNode : public rclcpp::Node{ rclcpp::Publisher::SharedPtr final_pose_pub; - std::unique_ptr tf_static_broadcaster; + std::unique_ptr tf_broadcaster; std::shared_ptr tf_listener{nullptr}; std::unique_ptr tf_buffer; diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index e36301f..fc8ff10 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -8,7 +8,7 @@ #include "utils/enumerators.hpp" FusionNode::FusionNode() : Node("fusion_node"){ - this->tf_static_broadcaster = std::make_unique(*this); + this->tf_broadcaster = std::make_unique(*this); this->tf_buffer = std::make_unique(this->get_clock()); this->tf_listener = std::make_shared(*tf_buffer); @@ -124,7 +124,7 @@ void FusionNode::initCallback(const geometry_msgs::msg::PoseWithCovariance & ini init_tf_msg.rotation.z = init_msg.pose.orientation.z; init_tf_msg.rotation.w = init_msg.pose.orientation.w; - utils::updateStaticTransform(*this->tf_static_broadcaster, stamp, this->global_frame, this->robot_parent_frame, init_tf_msg); + utils::updateTransform(*this->tf_broadcaster, stamp, this->global_frame, this->robot_parent_frame, init_tf_msg); } void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ @@ -209,7 +209,7 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); bool tf_available = utils::indirectUpdateTransform( - *this->tf_static_broadcaster, + *this->tf_broadcaster, *this->tf_buffer, rclcpp::Time(0), this->global_frame, @@ -264,7 +264,7 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); bool tf_available = utils::indirectUpdateTransform( - *this->tf_static_broadcaster, + *this->tf_broadcaster, *this->tf_buffer, stamp, this->global_frame, @@ -319,7 +319,7 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); bool tf_available = utils::indirectUpdateTransform( - *this->tf_static_broadcaster, + *this->tf_broadcaster, *this->tf_buffer, stamp, this->global_frame, diff --git a/devel/utils/include/utils/gen_functs.hpp b/devel/utils/include/utils/gen_functs.hpp index b4da4e0..58c53c0 100644 --- a/devel/utils/include/utils/gen_functs.hpp +++ b/devel/utils/include/utils/gen_functs.hpp @@ -17,6 +17,7 @@ geometry_msgs::msg::Quaternion yaw2qua(double); void updateStaticTransform(tf2_ros::StaticTransformBroadcaster &, rclcpp::Time, std::string, std::string, geometry_msgs::msg::Transform); void updateTransform(tf2_ros::TransformBroadcaster &, rclcpp::Time, std::string, std::string, geometry_msgs::msg::Transform); -bool indirectUpdateTransform(tf2_ros::StaticTransformBroadcaster &, tf2_ros::Buffer &, rclcpp::Time, std::string, std::string, std::string, geometry_msgs::msg::Transform); + +bool indirectUpdateTransform(tf2_ros::TransformBroadcaster &, tf2_ros::Buffer &, rclcpp::Time, std::string, std::string, std::string, geometry_msgs::msg::Transform); } \ No newline at end of file diff --git a/devel/utils/src/gen_functs.cpp b/devel/utils/src/gen_functs.cpp index 19bf962..b1139d7 100644 --- a/devel/utils/src/gen_functs.cpp +++ b/devel/utils/src/gen_functs.cpp @@ -60,7 +60,7 @@ void updateTransform( * A: from_frame, B: to_frame, C: by_frame, TF of AC: tf_msg. */ bool indirectUpdateTransform( - tf2_ros::StaticTransformBroadcaster & static_tf_br, + tf2_ros::TransformBroadcaster & tf_br, tf2_ros::Buffer & tf_buf, rclcpp::Time stamp, std::string from_frame, @@ -92,7 +92,7 @@ bool indirectUpdateTransform( from2to_tf_msg.transform = tf2::toMsg(from2to_tf); - static_tf_br.sendTransform(from2to_tf_msg); + tf_br.sendTransform(from2to_tf_msg); return true; } From 09d091b62f5cfd5daea5740ebcc3c7e686388f66 Mon Sep 17 00:00:00 2001 From: robert Date: Sun, 1 Mar 2026 21:30:03 +0800 Subject: [PATCH 10/22] Docs: launch files in localization system - add launch files for every localization layer - install `param` in CMakeLists.txt to use `find-pkg-share` in launch files - remove old launch files - rename `lidar_localization.yml` to `lidar_localization.yaml` for consistency - haven't refactor launch settings for rival localization --- devel/core_algorithm/fusion/CMakeLists.txt | 5 +- .../fusion/launch/b_running_launch.py | 191 ----------------- .../fusion/launch/bag_test.launch | 47 ----- .../core_algorithm/fusion/launch/blue.launch | 58 ------ .../fusion/launch/fake_cam.launch | 20 -- .../fusion/launch/fusion.launch.xml | 2 +- devel/core_algorithm/fusion/launch/run.launch | 38 ---- .../fusion/launch/y_running_launch.py | 192 ----------------- .../fusion/launch/yellow.launch | 56 ----- .../global/lidar_localization/CMakeLists.txt | 5 +- .../launch/calibrator_launch.xml | 5 - .../launch/debug_visualize_launch.xml | 32 --- .../launch/lidar_localization.launch.xml | 22 ++ .../launch/radius_compensation_launch.xml | 20 -- .../launch/view_obstacles_launch.xml | 11 - ...calization.yml => lidar_localization.yaml} | 0 .../local/imu_drive/launch/imu_drive_firm.xml | 1 - .../wheel_inertial_odometry/CMakeLists.txt | 1 + .../launch/local.launch.py | 81 -------- .../launch/local.launch.xml | 22 ++ .../launch/local_filter.xml | 39 ---- .../launch/local_filter_whole.xml | 63 ------ .../launch/wio.launch.xml | 12 ++ .../param/local_filter.yaml | 117 ----------- .../param/local_filter_whole.yaml | 196 ------------------ .../wheel_inertial_odometry/param/wio.yaml | 12 ++ devel/localization_bringup/CMakeLists.txt | 5 +- .../launch/localization.launch.py | 138 ++++++------ 28 files changed, 148 insertions(+), 1243 deletions(-) delete mode 100644 devel/core_algorithm/fusion/launch/b_running_launch.py delete mode 100644 devel/core_algorithm/fusion/launch/bag_test.launch delete mode 100644 devel/core_algorithm/fusion/launch/blue.launch delete mode 100644 devel/core_algorithm/fusion/launch/fake_cam.launch delete mode 100644 devel/core_algorithm/fusion/launch/run.launch delete mode 100644 devel/core_algorithm/fusion/launch/y_running_launch.py delete mode 100644 devel/core_algorithm/fusion/launch/yellow.launch delete mode 100644 devel/core_algorithm/global/lidar_localization/launch/calibrator_launch.xml delete mode 100644 devel/core_algorithm/global/lidar_localization/launch/debug_visualize_launch.xml create mode 100644 devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml delete mode 100644 devel/core_algorithm/global/lidar_localization/launch/radius_compensation_launch.xml delete mode 100644 devel/core_algorithm/global/lidar_localization/launch/view_obstacles_launch.xml rename devel/core_algorithm/global/lidar_localization/param/{lidar_localization.yml => lidar_localization.yaml} (100%) delete mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py create mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml delete mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter.xml delete mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter_whole.xml create mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/launch/wio.launch.xml delete mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter.yaml delete mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter_whole.yaml create mode 100644 devel/core_algorithm/local/wheel_inertial_odometry/param/wio.yaml diff --git a/devel/core_algorithm/fusion/CMakeLists.txt b/devel/core_algorithm/fusion/CMakeLists.txt index 8929f86..d459ae6 100644 --- a/devel/core_algorithm/fusion/CMakeLists.txt +++ b/devel/core_algorithm/fusion/CMakeLists.txt @@ -52,8 +52,9 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -install( - DIRECTORY launch +install(DIRECTORY + launch + param DESTINATION share/${PROJECT_NAME} ) diff --git a/devel/core_algorithm/fusion/launch/b_running_launch.py b/devel/core_algorithm/fusion/launch/b_running_launch.py deleted file mode 100644 index 683620b..0000000 --- a/devel/core_algorithm/fusion/launch/b_running_launch.py +++ /dev/null @@ -1,191 +0,0 @@ -#!/usr/bin/env python3 -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetLaunchConfiguration, TimerAction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - rival_name = LaunchConfiguration('rival_name') - side = LaunchConfiguration('side') - - local_filter_launch_path = PathJoinSubstitution([ - FindPackageShare('wheel_inertial_odometry'), - 'launch', - 'local.launch.py' - ]) - - # rplidar_launch = PathJoinSubstitution([ - # FindPackageShare('lidar_localization_pkg'), - # 'launch', - # 'firmware', - # 'rplidar_s3_launch.py' - # ]) - ydlidar_launch = PathJoinSubstitution([ - FindPackageShare('lidar_localization'), - 'launch', - 'firmware', - 'ydlidar_launch.py' - ]) - - obstacle_extractor_launch = PathJoinSubstitution([ - FindPackageShare('lidar_localization'), - 'launch', - 'obstacle_extractor_launch.xml' - ]) - -# healthcheck_node = Node( -# package='healthcheck', -# executable='healthcheck_node', -# name='healthcheck_node', -# output='screen' - # ,remappings=[ - # ['/vision/aruco/robot/single/average_pose', '/vision/aruco/robot_pose'] - # ] -# ) - - ekf_node = Node( - package='fusion', - executable='ekf_node.py', - name='ekf', - output='screen', - parameters=[{ - 'use_cam': 0, - 'robot_parent_frame_id': 'map', - 'robot_frame_id': 'base_footprint', - '/use_sim_time': False, - 'q_linear': 1.2e-6, - 'q_angular': 1.7e-7, - 'r_camra_linear': 1e-2, - 'r_camra_angular': 0.1, - 'r_threshold_xy': 1e-2, - 'r_threshold_theta': 1e-1 - }], - ) - - lidar_node = Node( - package='lidar_localization', - executable='lidar_member_function.py', - name='lidar_localization_node', - arguments=['--ros-args', '--log-level', 'info'], - output='screen', - parameters=[{ - 'side': side, - 'debug_mode': False, - 'visualize_candidate': True, - 'likelihood_threshold': 0.8, - 'consistency_threshold': 0.95, - 'lidar_multiplier': 0.987 - }] - ) - - local_filter_launch = IncludeLaunchDescription( - AnyLaunchDescriptionSource(local_filter_launch_path) - ) - - rival_node = Node( - package='rival_localization', - executable='rival_localization', - name='rival_localization', - output='screen', - parameters=[ - { - 'robot_name': 'robot', - 'frequency': 10.0, - 'x_max': 3.0, - 'x_min': 0.0, - 'y_max': 2.0, - 'y_min': 0.0, - 'vel_lpf_gain': 0.9, - 'locking_rad': 0.3, - 'lockrad_growing_rate': 0.3, - 'is_me': 0.3, - 'cam_weight': 6.0, - 'obs_weight': 10.0, - 'side_weight': 2.0, - 'cam_side_threshold': 0.2, - 'side_obs_threshold': 0.2, - 'obs_cam_threshold': 0.2, - 'crossed_areas': [ - 2.55, 3.0, 0.65, 1.1, - 2.4, 2.85, 1.55, 2.0, - 2.0, 3.0, 0.0, 0.15, - 1.0, 2.0, 0.0, 0.4, - 0.0, 1.0, 0.0, 0.15, - 0.0, 0.45, 0.65, 1.1, - 0.15, 0.6, 1.55, 2.0, - 1.05, 1.95, 1.50, 2.0 - ] - } - ], - remappings=[ - ('raw_pose', PathJoinSubstitution([rival_name, '/raw_pose'])), - ('/ceiling_rival/pose','/vision/aruco/rival_pose') - ] - ) - - rival_obstacle_node = Node( - package='obstacle_detector', - executable='obstacle_extractor_node', - name='obstacle_detector_to_robot', - parameters=[{ - 'frame_id': 'map', - 'active': True, - 'use_scan': True, - 'use_pcl': False, - 'use_split_and_merge': True, - 'circles_from_visibles': True, - 'discard_converted_segments': False, - 'transform_coordinates': True, - 'min_group_points': 5, - 'max_group_distance': 0.04, - 'distance_proportion': 0.00628, - 'max_split_distance': 0.02, - 'max_merge_separation': 0.25, - 'max_merge_spread': 0.02, - 'max_circle_radius': 0.2, - 'radius_enlargement': 0.05, - 'pose_array': True - }], - remappings=[ - ('raw_obstacles', '/obstacles_to_map'), - ('scan', '/scan'), - ('raw_obstacles_visualization_pcl', 'raw_obstacle_visualization_to_map_pcl') - ] - ) - - static_tf = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='robot_to_laser', - output='screen', - arguments=[ - '--x', '0', '--y', '0', '--z', '0', - '--roll', '0', '--pitch', '0', '--yaw', '-1.633628', - '--frame-id', 'base_footprint', - '--child-frame-id', 'laser' - ] - ) - - # rplidar_include = IncludeLaunchDescription(PythonLaunchDescriptionSource(rplidar_launch)) - ydlidar_include = IncludeLaunchDescription(PythonLaunchDescriptionSource(ydlidar_launch)) - obstacle_extractor_include = IncludeLaunchDescription(AnyLaunchDescriptionSource(obstacle_extractor_launch)) - - return LaunchDescription([ - DeclareLaunchArgument('rival_name', default_value='rival'), - DeclareLaunchArgument('side', default_value='1'), - - static_tf, - ydlidar_include, - obstacle_extractor_include, - - ekf_node, -# healthcheck_node, - - # TimerAction(period=2.0, actions=[ekf_node]), - TimerAction(period=4.0, actions=[lidar_node]), - TimerAction(period=6.0, actions=[local_filter_launch]), - TimerAction(period=8.0, actions=[rival_node, rival_obstacle_node]) - ]) \ No newline at end of file diff --git a/devel/core_algorithm/fusion/launch/bag_test.launch b/devel/core_algorithm/fusion/launch/bag_test.launch deleted file mode 100644 index 617a0dc..0000000 --- a/devel/core_algorithm/fusion/launch/bag_test.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/fusion/launch/blue.launch b/devel/core_algorithm/fusion/launch/blue.launch deleted file mode 100644 index 07f8334..0000000 --- a/devel/core_algorithm/fusion/launch/blue.launch +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/fusion/launch/fake_cam.launch b/devel/core_algorithm/fusion/launch/fake_cam.launch deleted file mode 100644 index fd04253..0000000 --- a/devel/core_algorithm/fusion/launch/fake_cam.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/fusion/launch/fusion.launch.xml b/devel/core_algorithm/fusion/launch/fusion.launch.xml index 6f93f56..f1e2ca7 100644 --- a/devel/core_algorithm/fusion/launch/fusion.launch.xml +++ b/devel/core_algorithm/fusion/launch/fusion.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/devel/core_algorithm/fusion/launch/run.launch b/devel/core_algorithm/fusion/launch/run.launch deleted file mode 100644 index f40af4d..0000000 --- a/devel/core_algorithm/fusion/launch/run.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/fusion/launch/y_running_launch.py b/devel/core_algorithm/fusion/launch/y_running_launch.py deleted file mode 100644 index b298b1b..0000000 --- a/devel/core_algorithm/fusion/launch/y_running_launch.py +++ /dev/null @@ -1,192 +0,0 @@ -#!/usr/bin/env python3 -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetLaunchConfiguration, TimerAction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - rival_name = LaunchConfiguration('rival_name') - side = LaunchConfiguration('side') - - fusion_launch_path = PathJoinSubstitution([ - FindPackageShare('fusion'), - 'launch', - 'fusion.launch.xml' - ]) - - local_filter_launch_path = PathJoinSubstitution([ - FindPackageShare('wheel_inertial_odometry'), - 'launch', - 'local.launch.py' - ]) - - # rplidar_launch = PathJoinSubstitution([ - # FindPackageShare('lidar_localization_pkg'), - # 'launch', - # 'firmware', - # 'rplidar_s3_launch.py' - # ]) - ydlidar_launch = PathJoinSubstitution([ - FindPackageShare('lidar_localization'), - 'launch', - 'firmware', - 'ydlidar_launch.py' - ]) - - obstacle_extractor_launch = PathJoinSubstitution([ - FindPackageShare('lidar_localization'), - 'launch', - 'obstacle_extractor_launch.xml' - ]) - -# healthcheck_node = Node( -# package='healthcheck', -# executable='healthcheck_node', -# name='healthcheck_node', -# output='screen' -# # ,remappings=[ -# # ['/vision/aruco/robot/single/average_pose', '/vision/aruco/robot_pose'] -# # ] -# ) - - init_pose = Node( - package='fusion', - executable='pub_init', - name='pub_init', - output='screen', - parameters=[ - '/home/user/localization-ws/src/core_algorithm/fusion/param/fusion.yaml' - ] - ) - - lidar_node = Node( - package='lidar_localization', - executable='lidar_member_function.py', - name='lidar_localization_node', - arguments=['--ros-args', '--log-level', 'info'], - output='screen', - parameters=[{ - 'side': side, - 'debug_mode': False, - 'visualize_candidate': True, - 'likelihood_threshold': 0.8, - 'consistency_threshold': 0.95, - 'lidar_multiplier': 0.987 - }] - ) - - fusion_launch = IncludeLaunchDescription( - AnyLaunchDescriptionSource(fusion_launch_path) - ) - - local_filter_launch = IncludeLaunchDescription( - AnyLaunchDescriptionSource(local_filter_launch_path) - ) - - rival_node = Node( - package='rival_localization', - executable='rival_localization', - name='rival_localization', - output='screen', - parameters=[ - { - 'robot_name': 'robot', - 'frequency': 10.0, - 'x_max': 3.0, - 'x_min': 0.0, - 'y_max': 2.0, - 'y_min': 0.0, - 'vel_lpf_gain': 0.9, - 'locking_rad': 0.3, - 'lockrad_growing_rate': 0.3, - 'is_me': 0.3, - 'cam_weight': 6.0, - 'obs_weight': 10.0, - 'side_weight': 2.0, - 'cam_side_threshold': 0.2, - 'side_obs_threshold': 0.2, - 'obs_cam_threshold': 0.2, - 'crossed_areas': [ - 2.55, 3.0, 0.65, 1.1, - 2.4, 2.85, 1.55, 2.0, - 2.0, 3.0, 0.0, 0.15, - 1.0, 2.0, 0.0, 0.4, - 0.0, 1.0, 0.0, 0.15, - 0.0, 0.45, 0.65, 1.1, - 0.15, 0.6, 1.55, 2.0, - 1.05, 1.95, 1.50, 2.0 - ] - } - ], - remappings=[ - ('raw_pose', PathJoinSubstitution([rival_name, '/raw_pose'])), - ('/ceiling_rival/pose','/vision/aruco/rival_pose') - ] - ) - - rival_obstacle_node = Node( - package='obstacle_detector', - executable='obstacle_extractor_node', - name='obstacle_detector_to_robot', - parameters=[{ - 'frame_id': 'map', - 'active': True, - 'use_scan': True, - 'use_pcl': False, - 'use_split_and_merge': True, - 'circles_from_visibles': True, - 'discard_converted_segments': False, - 'transform_coordinates': True, - 'min_group_points': 5, - 'max_group_distance': 0.04, - 'distance_proportion': 0.00628, - 'max_split_distance': 0.02, - 'max_merge_separation': 0.25, - 'max_merge_spread': 0.02, - 'max_circle_radius': 0.2, - 'radius_enlargement': 0.05, - 'pose_array': True - }], - remappings=[ - ('raw_obstacles', '/obstacles_to_map'), - ('scan', '/scan'), - ('raw_obstacles_visualization_pcl', 'raw_obstacle_visualization_to_map_pcl') - ] - ) - - static_tf = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='robot_to_laser', - output='screen', - arguments=[ - '--x', '0', '--y', '0', '--z', '0', - '--roll', '0', '--pitch', '0', '--yaw', '-1.633628', - '--frame-id', 'base_footprint', - '--child-frame-id', 'laser' - ] - ) - - # rplidar_include = IncludeLaunchDescription(PythonLaunchDescriptionSource(rplidar_launch)) - ydlidar_include = IncludeLaunchDescription(PythonLaunchDescriptionSource(ydlidar_launch)) - obstacle_extractor_include = IncludeLaunchDescription(AnyLaunchDescriptionSource(obstacle_extractor_launch)) - - return LaunchDescription([ - DeclareLaunchArgument('rival_name', default_value='rival'), - DeclareLaunchArgument('side', default_value='0'), - - static_tf, - ydlidar_include, - obstacle_extractor_include, - -# healthcheck_node, - - TimerAction(period=0.5, actions=[init_pose]), - TimerAction(period=1.0, actions=[fusion_launch]), - TimerAction(period=1.5, actions=[lidar_node]), - TimerAction(period=2.0, actions=[local_filter_launch]), - TimerAction(period=2.5, actions=[rival_node, rival_obstacle_node]) - ]) \ No newline at end of file diff --git a/devel/core_algorithm/fusion/launch/yellow.launch b/devel/core_algorithm/fusion/launch/yellow.launch deleted file mode 100644 index 9c7f222..0000000 --- a/devel/core_algorithm/fusion/launch/yellow.launch +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/CMakeLists.txt b/devel/core_algorithm/global/lidar_localization/CMakeLists.txt index f4b4f59..231d527 100644 --- a/devel/core_algorithm/global/lidar_localization/CMakeLists.txt +++ b/devel/core_algorithm/global/lidar_localization/CMakeLists.txt @@ -33,8 +33,9 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) -install( - DIRECTORY launch +install(DIRECTORY + launch + param DESTINATION share/${PROJECT_NAME} ) diff --git a/devel/core_algorithm/global/lidar_localization/launch/calibrator_launch.xml b/devel/core_algorithm/global/lidar_localization/launch/calibrator_launch.xml deleted file mode 100644 index e319ef5..0000000 --- a/devel/core_algorithm/global/lidar_localization/launch/calibrator_launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/launch/debug_visualize_launch.xml b/devel/core_algorithm/global/lidar_localization/launch/debug_visualize_launch.xml deleted file mode 100644 index 977f540..0000000 --- a/devel/core_algorithm/global/lidar_localization/launch/debug_visualize_launch.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml new file mode 100644 index 0000000..0198ff6 --- /dev/null +++ b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/launch/radius_compensation_launch.xml b/devel/core_algorithm/global/lidar_localization/launch/radius_compensation_launch.xml deleted file mode 100644 index 25ef3b3..0000000 --- a/devel/core_algorithm/global/lidar_localization/launch/radius_compensation_launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/launch/view_obstacles_launch.xml b/devel/core_algorithm/global/lidar_localization/launch/view_obstacles_launch.xml deleted file mode 100644 index facabd6..0000000 --- a/devel/core_algorithm/global/lidar_localization/launch/view_obstacles_launch.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yaml similarity index 100% rename from devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml rename to devel/core_algorithm/global/lidar_localization/param/lidar_localization.yaml diff --git a/devel/core_algorithm/local/imu_drive/launch/imu_drive_firm.xml b/devel/core_algorithm/local/imu_drive/launch/imu_drive_firm.xml index 0015644..1b25bae 100644 --- a/devel/core_algorithm/local/imu_drive/launch/imu_drive_firm.xml +++ b/devel/core_algorithm/local/imu_drive/launch/imu_drive_firm.xml @@ -19,7 +19,6 @@ - diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/CMakeLists.txt b/devel/core_algorithm/local/wheel_inertial_odometry/CMakeLists.txt index d8e1260..599b777 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/CMakeLists.txt +++ b/devel/core_algorithm/local/wheel_inertial_odometry/CMakeLists.txt @@ -60,6 +60,7 @@ install(DIRECTORY install(DIRECTORY launch + param DESTINATION share/${PROJECT_NAME} ) diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py deleted file mode 100644 index 1c1cd51..0000000 --- a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(): - robot_name = LaunchConfiguration('robot_name') - robot_name_slash = LaunchConfiguration('robot_name_slash') - imu_param = LaunchConfiguration('imu_param') - simulation = LaunchConfiguration('simulation') - linear_velocity_threshold = LaunchConfiguration('linear_velocity_threshold') - angular_velocity_threshold = LaunchConfiguration('angular_velocity_threshold') - robot_parent_frame_id = LaunchConfiguration('robot_parent_frame_id') - robot_frame_id = LaunchConfiguration('robot_frame_id') - vx_coeff = LaunchConfiguration('vx_coeff') - vy_coeff = LaunchConfiguration('vy_coeff') - w_coeff = LaunchConfiguration('w_coeff') - x_cov_min = LaunchConfiguration('x_cov_min') - y_cov_min = LaunchConfiguration('y_cov_min') - theta_cov_min = LaunchConfiguration('theta_cov_min') - - declare_args = [ - DeclareLaunchArgument('robot_name', default_value='robot'), - DeclareLaunchArgument('robot_name_slash', default_value='robot/'), - DeclareLaunchArgument('imu_param', default_value='0'), - DeclareLaunchArgument('simulation', default_value='false'), - DeclareLaunchArgument('linear_velocity_threshold', default_value='2.0'), - DeclareLaunchArgument('angular_velocity_threshold', default_value='6.0'), - DeclareLaunchArgument('robot_parent_frame_id', default_value='odom'), - DeclareLaunchArgument('robot_frame_id', default_value='base_footprint'), - DeclareLaunchArgument('vx_coeff', default_value='0.3'), - DeclareLaunchArgument('vy_coeff', default_value='0.3'), - DeclareLaunchArgument('w_coeff', default_value='2.0'), - DeclareLaunchArgument('x_cov_min', default_value='1.67e-6'), - DeclareLaunchArgument('y_cov_min', default_value='1.67e-6'), - DeclareLaunchArgument('theta_cov_min', default_value='1.07e-4'), - ] - - # IMU firmware and covariance feedback loop node (contain IMU spatial launch) - imu_drive_launch_path = PathJoinSubstitution([ - FindPackageShare('imu_drive'), - 'launch', - 'imu_drive_firm.xml' - ]) - - imu_drive_launch = IncludeLaunchDescription( - AnyLaunchDescriptionSource(imu_drive_launch_path), - launch_arguments={ - 'robot_name': robot_name, - 'robot_name_slash': robot_name_slash, - 'param': imu_param, - 'simulation': simulation, - }.items() - ) - - wio_node = Node( - package='wheel_inertial_odometry', - executable='wio_node', - name='wio_node', - parameters=[{ - 'linear_velocity_threshold': linear_velocity_threshold, - 'angular_velocity_threshold': angular_velocity_threshold, - 'robot_parent_frame_id': robot_parent_frame_id, - 'robot_frame_id': robot_frame_id, - 'vx_coeff': vx_coeff, - 'vy_coeff': vy_coeff, - 'w_coeff': w_coeff, - 'x_cov_min': x_cov_min, - 'y_cov_min': y_cov_min, - 'theta_cov_min': theta_cov_min, - }] - ) - - return LaunchDescription( - declare_args + [ - wio_node, - imu_drive_launch, - ] - ) \ No newline at end of file diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml new file mode 100644 index 0000000..56826ad --- /dev/null +++ b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter.xml b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter.xml deleted file mode 100644 index 2894e42..0000000 --- a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter_whole.xml b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter_whole.xml deleted file mode 100644 index 088f9a2..0000000 --- a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local_filter_whole.xml +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/wio.launch.xml b/devel/core_algorithm/local/wheel_inertial_odometry/launch/wio.launch.xml new file mode 100644 index 0000000..9b130a9 --- /dev/null +++ b/devel/core_algorithm/local/wheel_inertial_odometry/launch/wio.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter.yaml b/devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter.yaml deleted file mode 100644 index aebf2a8..0000000 --- a/devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter.yaml +++ /dev/null @@ -1,117 +0,0 @@ - -/**: - ros__parameters: - frequency: 100 - silent_tf_failure: false - sensor_timeout: 0.01 - two_d_mode: true - - smooth_lagged_data: false - history_length: 0.4 - - dynamic_process_noise_covariance: false - - predict_to_current_time: false - - print_diagnostics: false - - # debug - debug: false - debug_out_file: /path/to/debug/file.txt - - # frames - map_frame: map - odom_frame: odom - base_link_frame: base_footprint - world_frame: odom - transform_time_offset: 0.05 - transform_timeout: 0 - - # sensors - # config: [x, y, z, - # r, p, y, - # x., y., z., - # r., p., y., - # x.., y.., z..] - odom0: odom - odom0_config: [false, false, false, - false, false, false, - true, true, false, - false, false, true, - false, false, false] - odom0_queue_size: 20 - odom0_differential: false - odom0_relative: false - odom0_nodelay: true - - odom0_twist_rejection_threshold: 100 - - imu0: imu/data_cov - imu0_config: [false, false, false, - false, false, false, - false, false, false, - false, false, true, - true, true, false] - imu0_queue_size: 100 - imu0_differential: false - imu0_relative: false - imu0_nodelay: true - - imu0_twist_rejection_threshold: 1000000000000.0 - imu0_linear_acceleration_rejection_threshold: 1000000000000.0 - - imu0_remove_gravitational_acceleration: false - - - initial_state: [0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0] - - publish_tf: true - publish_acceleration: true - - # control - # acc_config: [x., y., z., r., p., y.] - use_control: false - stamped_control: false - control_timeout: 0.2 - control_config: [true, true, false, false, false, true] - acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] - deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] - acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] - deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] - - # config: [x, y, z, r, p, y, x., y., z., r., p., y., x.., y.., z..] - process_noise_covariance: [0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] - - initial_estimate_covariance: [0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter_whole.yaml b/devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter_whole.yaml deleted file mode 100644 index 0f870b5..0000000 --- a/devel/core_algorithm/local/wheel_inertial_odometry/param/local_filter_whole.yaml +++ /dev/null @@ -1,196 +0,0 @@ -# --------------------------- -# -------- Rosserial -------- -# --------------------------- -rosserial_server_dp: - ros__parameters: - port: "/dev/USB0-3" # not used? - -# --------------------------- -# ------ Local filter ndoe ------ -# --------------------------- -local_filter_LPF: - ros__parameters: - # Active this node - active: true - - # Set publish to this node - publish: true - - # Open param service - update_params: false - - # Covariance for "const" part - covariance_x: 0.001 - covariance_y: 0.001 - covariance_z: 0.001 - covariance_vx: 0.0042 - covariance_vy: 0.0042 - covariance_vz: 0.005 - - # Covariance for "multiple" part - covariance_multi_vx: 0. - covariance_multi_vy: 0. - covariance_multi_vz: 0. - - # Dynamic adjustment from : - # True: (ns)/cmd_vel - # False: (ns)/final_pose - using_nav_vel_cb: false - - # Open if STM has integral information - use_stm_integral: false - - # Open Dynamic reconfigure service - using_dynamic_reconf: true - -# ---------------------- -# ------ IMU node ------ -# ---------------------- -imu_node: - ros__parameters: - # Set the usage of the node - active: true - publish: true - - # Open param service - update_params: false - - # Covariance for "const" part - covariance_vx: 0.005 - covariance_vy: 0.005 - covariance_vz: 0.001 - covariance_ax: 0.1 - covariance_ay: 0.1 - covariance_az: 0.1 - - # Covariance for "multiple" part - cov_multi_vel: 0.2 - cov_multi_acl: 0.2 - - # Use which topic to dynamic adjust covariance - using_nav_vel_cb: false - - # Low pass filter -> beleive previous - filter_prev: 0.1 - - using_dynamic_reconf: true - -# ---------------------- -# ------ EKF node ------ -# ---------------------- -ekf_velocity: - ros__parameters: - frequency: 100 - silent_tf_failure: false - sensor_timeout: 0.01 - two_d_mode: true - - smooth_lagged_data: false - history_length: 0.4 - - dynamic_process_noise_covariance: false - - predict_to_current_time: false - - print_diagnostics: false - - # debug - debug: false - debug_out_file: /path/to/debug/file.txt - - # frames - map_frame: map - odom_frame: odom - base_link_frame: base_footprint - world_frame: odom - transform_time_offset: 0.0 - transform_timeout: 0 - - # sensors - # config: [x, y, z, - # r, p, y, - # x., y., z., - # r., p., y., - # x.., y.., z..] - odom0: odom - odom0_config: [false, false, false, - false, false, false, - true, true, false, - false, false, true, - false, false, false] - odom0_queue_size: 20 - odom0_differential: false - odom0_relative: false - odom0_nodelay: true - - odom0_twist_rejection_threshold: 100 - - imu0: imu/data_cov - imu0_config: [false, false, false, - false, false, false, - false, false, false, - false, false, true, - false, false, false] - imu0_queue_size: 100 - imu0_differential: false - imu0_relative: false - imu0_nodelay: true - - imu0_twist_rejection_threshold: 1000000000000.0 - imu0_linear_acceleration_rejection_threshold: 1000000000000.0 - - imu0_remove_gravitational_acceleration: false - - - initial_state: [0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0] - - publish_tf: true - publish_acceleration: true - - # control - # acc_config: [x., y., z., r., p., y.] - use_control: false - stamped_control: false - control_timeout: 0.2 - control_config: [true, true, false, false, false, true] - acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] - deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] - acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] - deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] - - # config: [x, y, z, r, p, y, x., y., z., r., p., y., x.., y.., z..] - process_noise_covariance: [1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1e-2, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1e-2, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] - - initial_estimate_covariance: [0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-4, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-4, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/param/wio.yaml b/devel/core_algorithm/local/wheel_inertial_odometry/param/wio.yaml new file mode 100644 index 0000000..a54538e --- /dev/null +++ b/devel/core_algorithm/local/wheel_inertial_odometry/param/wio.yaml @@ -0,0 +1,12 @@ +wio_node: + ros__parameters: + linear_velocity_threshold: 2.0 + angular_velocity_threshold: 6.0 + + vx_coeff: 0.3 + vy_coeff: 0.3 + w_coeff: 2.0 + + x_cov_min: 1.67e-6 + y_cov_min: 1.67e-6 + theta_cov_min: 1.07e-4 \ No newline at end of file diff --git a/devel/localization_bringup/CMakeLists.txt b/devel/localization_bringup/CMakeLists.txt index 41f5781..39bab2e 100644 --- a/devel/localization_bringup/CMakeLists.txt +++ b/devel/localization_bringup/CMakeLists.txt @@ -24,8 +24,9 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -install( - DIRECTORY launch +install(DIRECTORY + launch + param DESTINATION share/${PROJECT_NAME} ) diff --git a/devel/localization_bringup/launch/localization.launch.py b/devel/localization_bringup/launch/localization.launch.py index 1230056..a09e409 100644 --- a/devel/localization_bringup/launch/localization.launch.py +++ b/devel/localization_bringup/launch/localization.launch.py @@ -7,41 +7,79 @@ def generate_launch_description(): + global_frame_id = LaunchConfiguration('global_frame_id') + robot_parent_frame_id = LaunchConfiguration('robot_parent_frame_id') + robot_frame_id = LaunchConfiguration('robot_frame_id') + mode = LaunchConfiguration('mode') rival_name = LaunchConfiguration('rival_name') - # Declare arguments - mode_arg = DeclareLaunchArgument( - 'mode', - default_value='yellow', - description='Determine how to pubish initial pose' - ) + declare_args = [ + DeclareLaunchArgument( + 'global_frame_id', + default_value='map' + ), + DeclareLaunchArgument( + 'robot_parent_frame_id', + default_value='odom' + ), + DeclareLaunchArgument( + 'robot_frame_id', + default_value='base_footprint' + ), + DeclareLaunchArgument( + 'mode', + default_value='yellow' + ), + DeclareLaunchArgument( + 'rival_name', + default_value='rival' + ), + ] - mode = LaunchConfiguration('mode') fusion_launch_path = PathJoinSubstitution([ - FindPackageShare('fusion'), - 'launch', + FindPackageShare('fusion'), + 'launch', 'fusion.launch.xml' ]) local_launch_path = PathJoinSubstitution([ - FindPackageShare('wheel_inertial_odometry'), - 'launch', - 'local.launch.py' + FindPackageShare('wheel_inertial_odometry'), + 'launch', + 'local.launch.xml' ]) - ydlidar_launch = PathJoinSubstitution([ - FindPackageShare('lidar_localization'), - 'launch', - 'firmware', - 'ydlidar_launch.py' + global_launch_path = PathJoinSubstitution([ + FindPackageShare('lidar_localization'), + 'launch', + 'lidar_localization.launch.xml' ]) - obstacle_extractor_launch = PathJoinSubstitution([ - FindPackageShare('lidar_localization'), - 'launch', - 'obstacle_extractor_launch.xml' - ]) + + fusion_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource(fusion_launch_path), + launch_arguments = { + 'global_frame_id': global_frame_id, + 'robot_parent_frame_id': robot_parent_frame_id, + 'robot_frame_id': robot_frame_id, + }.items() + ) + + local_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource(local_launch_path), + launch_arguments={ + 'robot_parent_frame_id': robot_parent_frame_id, + 'robot_frame_id': robot_frame_id, + }.items() + ) + + global_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource(global_launch_path), + launch_arguments={ + 'mode': mode + }.items() + ) + init_pose = Node( package='localization_bringup', @@ -49,31 +87,15 @@ def generate_launch_description(): name='pub_init', output='screen', parameters=[ - '/home/user/localization-ws/src/devel/localization_bringup/param/initial_pose.yaml', + PathJoinSubstitution([ + FindPackageShare('localization_bringup'), + 'param', + 'initial_pose.yaml' + ]), {'mode': mode} ] ) - lidar_node = Node( - package='lidar_localization', - executable='lidar_member_function.py', - name='lidar_localization_node', - arguments=['--ros-args', '--log-level', 'info'], - output='screen', - parameters=[ - '/home/user/localization-ws/src/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml', - {'mode': mode} - ] - ) - - fusion_launch = IncludeLaunchDescription( - AnyLaunchDescriptionSource(fusion_launch_path) - ) - - local_launch = IncludeLaunchDescription( - AnyLaunchDescriptionSource(local_launch_path) - ) - rival_node = Node( package='rival_localization', executable='rival_localization', @@ -145,34 +167,12 @@ def generate_launch_description(): ] ) - static_tf = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='robot_to_laser', - output='screen', - arguments=[ - '--x', '0', '--y', '0', '--z', '0', - '--roll', '0', '--pitch', '0', '--yaw', '3.14', - '--frame-id', 'base_footprint', - '--child-frame-id', 'laser' - ] - ) - - ydlidar_include = IncludeLaunchDescription(PythonLaunchDescriptionSource(ydlidar_launch)) - obstacle_extractor_include = IncludeLaunchDescription(AnyLaunchDescriptionSource(obstacle_extractor_launch)) - return LaunchDescription([ - DeclareLaunchArgument('rival_name', default_value='rival'), - DeclareLaunchArgument('side', default_value='0'), - - mode_arg, - ydlidar_include, - obstacle_extractor_include, + *declare_args, + TimerAction(period=2.0, actions=[local_launch]), + TimerAction(period=1.5, actions=[global_launch]), TimerAction(period=0.5, actions=[init_pose]), TimerAction(period=1.0, actions=[fusion_launch]), - TimerAction(period=1.5, actions=[lidar_node]), - TimerAction(period=2.0, actions=[local_launch]), - TimerAction(period=2.5, actions=[rival_node, rival_obstacle_node]), - TimerAction(period=3.0, actions=[static_tf]) + TimerAction(period=2.5, actions=[rival_node, rival_obstacle_node]) ]) \ No newline at end of file From 8582f28ab567c03ae446ed294ce767079f908bfc Mon Sep 17 00:00:00 2001 From: robert Date: Mon, 2 Mar 2026 22:28:11 +0800 Subject: [PATCH 11/22] Modify: look up latest transform instead of specific time --- devel/core_algorithm/fusion/src/fusion_node.cpp | 16 ++++++++-------- devel/utils/src/gen_functs.cpp | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index fc8ff10..2766590 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -146,7 +146,7 @@ void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ 0, local_msg.pose.covariance[7], 0, 0, 0, local_msg.pose.covariance[35]; - geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform("map", "odom", stamp); + geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform(this->global_frame, this->robot_parent_frame, tf2::TimePointZero); this->mapodom_state(0) = tf.transform.translation.x; this->mapodom_state(1) = tf.transform.translation.y; @@ -186,8 +186,8 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam // align timestamp of prediction and correction if(if_align){ rclcpp::Clock clock; - rclcpp::Time stamp = clock.now(); - double corr_time = stamp.nanoseconds()*1e-9; + rclcpp::Time align_stamp = clock.now(); + double corr_time = align_stamp.nanoseconds()*1e-9; double dt_align = corr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) @@ -211,7 +211,7 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam bool tf_available = utils::indirectUpdateTransform( *this->tf_broadcaster, *this->tf_buffer, - rclcpp::Time(0), + stamp, this->global_frame, this->robot_parent_frame, this->robot_frame, @@ -241,8 +241,8 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ // align timestamp of prediction and correction if(if_align){ rclcpp::Clock clock; - rclcpp::Time stamp = clock.now(); - double corr_time = stamp.nanoseconds()*1e-9; + rclcpp::Time align_stamp = clock.now(); + double corr_time = align_stamp.nanoseconds()*1e-9; double dt_align = corr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) @@ -296,8 +296,8 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ // align timestamp of prediction and correction if(if_align){ rclcpp::Clock clock; - rclcpp::Time stamp = clock.now(); - double corr_time = stamp.nanoseconds()*1e-9; + rclcpp::Time align_stamp = clock.now(); + double corr_time = align_stamp.nanoseconds()*1e-9; double dt_align = corr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) diff --git a/devel/utils/src/gen_functs.cpp b/devel/utils/src/gen_functs.cpp index b1139d7..591bd6f 100644 --- a/devel/utils/src/gen_functs.cpp +++ b/devel/utils/src/gen_functs.cpp @@ -62,7 +62,7 @@ void updateTransform( bool indirectUpdateTransform( tf2_ros::TransformBroadcaster & tf_br, tf2_ros::Buffer & tf_buf, - rclcpp::Time stamp, + rclcpp::Time stamp, std::string from_frame, std::string to_frame, std::string by_frame, @@ -81,7 +81,7 @@ bool indirectUpdateTransform( geometry_msgs::msg::TransformStamped to2by_tf_msg; try{ - to2by_tf_msg = tf_buf.lookupTransform(to_frame, by_frame, stamp); + to2by_tf_msg = tf_buf.lookupTransform(to_frame, by_frame, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { return false; } From 340221367f746432403328bc3cea7fd633560bd5 Mon Sep 17 00:00:00 2001 From: robert Date: Mon, 2 Mar 2026 22:52:54 +0800 Subject: [PATCH 12/22] Modify: time source get current time from ros time instead of system time, to make sure the time is consistent with ros messages. --- .../core_algorithm/fusion/src/fusion_node.cpp | 24 ++++++++----------- .../wheel_inertial_odometry/src/wio_node.cpp | 3 +-- 2 files changed, 11 insertions(+), 16 deletions(-) diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index 2766590..ec58d0d 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -95,8 +95,7 @@ FusionNode::FusionNode() : Node("fusion_node"){ void FusionNode::initCallback(const geometry_msgs::msg::PoseWithCovariance & init_msg){ - rclcpp::Clock clock; - rclcpp::Time stamp = clock.now(); + rclcpp::Time stamp = this->get_clock()->now(); this->init_state(0) = init_msg.pose.position.x; this->init_state(1) = init_msg.pose.position.y; @@ -185,10 +184,9 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam // align timestamp of prediction and correction if(if_align){ - rclcpp::Clock clock; - rclcpp::Time align_stamp = clock.now(); - double corr_time = align_stamp.nanoseconds()*1e-9; - double dt_align = corr_time-this->prev_pred_time; + rclcpp::Time align_stamp = this->get_clock()->now(); + double curr_time = align_stamp.nanoseconds()*1e-9; + double dt_align = curr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) ekf.Vprediction(this->local_twist, this->local_cov, dt_align); @@ -240,10 +238,9 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ // align timestamp of prediction and correction if(if_align){ - rclcpp::Clock clock; - rclcpp::Time align_stamp = clock.now(); - double corr_time = align_stamp.nanoseconds()*1e-9; - double dt_align = corr_time-this->prev_pred_time; + rclcpp::Time align_stamp = this->get_clock()->now(); + double curr_time = align_stamp.nanoseconds()*1e-9; + double dt_align = curr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) ekf.Vprediction(this->local_twist, this->local_cov, dt_align); @@ -295,10 +292,9 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ // align timestamp of prediction and correction if(if_align){ - rclcpp::Clock clock; - rclcpp::Time align_stamp = clock.now(); - double corr_time = align_stamp.nanoseconds()*1e-9; - double dt_align = corr_time-this->prev_pred_time; + rclcpp::Time align_stamp = this->get_clock()->now(); + double curr_time = align_stamp.nanoseconds()*1e-9; + double dt_align = curr_time-this->prev_pred_time; if(dt_alignmax_dt_align && dt_align>this->min_dt_align) ekf.Vprediction(this->local_twist, this->local_cov, dt_align); diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp index f75a603..1b1e916 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp +++ b/devel/core_algorithm/local/wheel_inertial_odometry/src/wio_node.cpp @@ -73,8 +73,7 @@ WioNode::WioNode() : Node("wio_node"){ void WioNode::wheelCallback(const geometry_msgs::msg::Twist & wheel_msg) { - rclcpp::Clock clock; - rclcpp::Time stamp = clock.now(); + rclcpp::Time stamp = this->get_clock()->now(); double odom_time = stamp.nanoseconds() * 1e-9; if (this->first_odom_cb) From 4e94cd84aaa71ffed3d7af5f6be423e9a40ac3b2 Mon Sep 17 00:00:00 2001 From: robert Date: Mon, 2 Mar 2026 23:13:00 +0800 Subject: [PATCH 13/22] Add: spin mode --- .../core_algorithm/fusion/include/fusion/fusion_node.hpp | 3 +++ devel/core_algorithm/fusion/param/fusion.yaml | 3 +++ devel/core_algorithm/fusion/src/fusion_node.cpp | 9 +++++++++ 3 files changed, 15 insertions(+) diff --git a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp index 8e67514..8a6f20b 100644 --- a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp +++ b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp @@ -57,9 +57,12 @@ class FusionNode : public rclcpp::Node{ Eigen::Matrix3d mapodom_rot; bool if_align; + bool spin_control; int pose_modes[4]; + double spin_threshold; + double max_dt_align; double min_dt_align; diff --git a/devel/core_algorithm/fusion/param/fusion.yaml b/devel/core_algorithm/fusion/param/fusion.yaml index 069c0cb..6ed4c6c 100644 --- a/devel/core_algorithm/fusion/param/fusion.yaml +++ b/devel/core_algorithm/fusion/param/fusion.yaml @@ -1,6 +1,9 @@ fusion_node: ros__parameters: align_timestamp: false + spin_control: true + + spin_threshold: 1.2 max_dt_align: 0.3 min_dt_align: 0.05 diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index ec58d0d..7e4f91c 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -40,6 +40,12 @@ FusionNode::FusionNode() : Node("fusion_node"){ this->declare_parameter("align_timestamp", false); this->if_align = this->get_parameter("align_timestamp").as_bool(); + this->declare_parameter("spin_control", false); + this->spin_control = this->get_parameter("spin_control").as_bool(); + + this->declare_parameter("spin_threshold", 1.2); + this->spin_threshold = this->get_parameter("spin_threshold").as_double(); + this->declare_parameter("max_dt_align", 0.3); this->max_dt_align = this->get_parameter("max_dt_align").as_double(); @@ -168,6 +174,9 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam if(this->pose_modes[static_cast(PoseEnum::GLOBAL)]==static_cast(ModeEnum::FREEZE)) return; + if(this->spin_control && this->local_twist(2)>this->spin_threshold) return; + + rclcpp::Time stamp = global_msg.header.stamp; From 9e6eac8bfbac910f9938953103f035aa0e53132e Mon Sep 17 00:00:00 2001 From: robert Date: Wed, 4 Mar 2026 15:50:03 +0800 Subject: [PATCH 14/22] Modify: lidar parameter, camera yaw - use `lidar_multiplier` parameter to calibrate lidar scan range - add adaptive measurement covariance to ignore camera yaw - assign lidar yaw to camera yaw when ekf update --- .../fusion/include/fusion/ekf.hpp | 2 +- .../fusion/include/fusion/fusion_node.hpp | 5 +++++ devel/core_algorithm/fusion/param/fusion.yaml | 12 +++++----- devel/core_algorithm/fusion/src/ekf.cpp | 6 ++--- .../core_algorithm/fusion/src/fusion_node.cpp | 22 ++++++++++++++----- .../param/lidar_localization.yml | 2 +- .../scripts/lidar_member_function.py | 6 ++--- 7 files changed, 36 insertions(+), 19 deletions(-) diff --git a/devel/core_algorithm/fusion/include/fusion/ekf.hpp b/devel/core_algorithm/fusion/include/fusion/ekf.hpp index efed7b2..7505bf0 100644 --- a/devel/core_algorithm/fusion/include/fusion/ekf.hpp +++ b/devel/core_algorithm/fusion/include/fusion/ekf.hpp @@ -17,7 +17,7 @@ class EKF{ void Xprediction(Eigen::Vector3d, Eigen::Matrix3d); void Vprediction(Eigen::Vector3d, Eigen::Matrix3d, double); - void correction(Eigen::Vector3d, Eigen::Matrix3d); + void correction(Eigen::Vector3d, Eigen::Matrix3d, Eigen::Matrix3d); Eigen::Vector3d getPose(); Eigen::Matrix3d getCov(); diff --git a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp index 8a6f20b..8058161 100644 --- a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp +++ b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp @@ -55,6 +55,9 @@ class FusionNode : public rclcpp::Node{ Eigen::Matrix3d obcam_cov; Eigen::Matrix3d init_rot; Eigen::Matrix3d mapodom_rot; + Eigen::Matrix3d global_h; + Eigen::Matrix3d cbcam_h; + Eigen::Matrix3d obcam_h; bool if_align; bool spin_control; @@ -65,6 +68,8 @@ class FusionNode : public rclcpp::Node{ double max_dt_align; double min_dt_align; + + double lidar_yaw; double prev_pred_time; diff --git a/devel/core_algorithm/fusion/param/fusion.yaml b/devel/core_algorithm/fusion/param/fusion.yaml index 6ed4c6c..5784815 100644 --- a/devel/core_algorithm/fusion/param/fusion.yaml +++ b/devel/core_algorithm/fusion/param/fusion.yaml @@ -8,10 +8,10 @@ fusion_node: max_dt_align: 0.3 min_dt_align: 0.05 - cb_cov_x: 4e-4 - cb_cov_y: 4e-4 - cb_cov_yaw: 4e-4 + cb_cov_x: 6.4e-3 + cb_cov_y: 6.4e-3 + cb_cov_yaw: 0.2739 - ob_cov_x: 4e-4 - ob_cov_y: 4e-4 - ob_cov_yaw: 4e-4 \ No newline at end of file + ob_cov_x: 6.4e-3 + ob_cov_y: 6.4e-3 + ob_cov_yaw: 0.2739 \ No newline at end of file diff --git a/devel/core_algorithm/fusion/src/ekf.cpp b/devel/core_algorithm/fusion/src/ekf.cpp index b361b3e..a56e410 100644 --- a/devel/core_algorithm/fusion/src/ekf.cpp +++ b/devel/core_algorithm/fusion/src/ekf.cpp @@ -43,14 +43,14 @@ void EKF::Vprediction(Eigen::Vector3d u, Eigen::Matrix3d R, double dt){ this->robot_state.sigma += R; } -void EKF::correction(Eigen::Vector3d z, Eigen::Matrix3d Q){ +void EKF::correction(Eigen::Vector3d z, Eigen::Matrix3d H, Eigen::Matrix3d Q){ Eigen::Matrix3d K; - K = this->robot_state.sigma*(this->robot_state.sigma+Q).inverse(); + K = this->robot_state.sigma*H.transpose()*(H*this->robot_state.sigma*H.transpose()+Q).inverse(); this->robot_state.mu += K*(z-this->robot_state.mu); Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); - this->robot_state.sigma = (I-K)*this->robot_state.sigma; + this->robot_state.sigma = (I-K*H)*this->robot_state.sigma; } Eigen::Vector3d EKF::getPose(){ diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index 7e4f91c..8d9709d 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -18,11 +18,21 @@ FusionNode::FusionNode() : Node("fusion_node"){ this->cbcam_cov.setIdentity(); this->obcam_cov.setIdentity(); this->init_rot.setIdentity(); + this->global_h.setIdentity(); + this->cbcam_h.setIdentity(); + this->obcam_h.setIdentity(); + + this->lidar_yaw = 0.0; for(int i=0; i<4; i++){ this->pose_modes[i] = static_cast(ModeEnum::ACTIVE); } + + this->cbcam_h(2, 2) = 0.0; + this->obcam_h(2, 2) = 0.0; + + rclcpp::QoS qos(1); qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); @@ -185,6 +195,8 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam double global_yaw = utils::qua2yaw(global_msg.pose.pose.orientation); global_pose << global_msg.pose.pose.position.x, global_msg.pose.pose.position.y, global_yaw; + + this->lidar_yaw = global_pose(2); Eigen::Matrix3d global_cov; global_cov << global_msg.pose.covariance[0], 0, 0, @@ -201,7 +213,7 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam ekf.Vprediction(this->local_twist, this->local_cov, dt_align); } - ekf.correction(global_pose, global_cov); + ekf.correction(global_pose, this->global_h, global_cov); Eigen::Vector3d robot_pose = ekf.getPose(); @@ -243,7 +255,7 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ double cbcam_yaw = utils::qua2yaw(cbcam_msg.pose.orientation); - cbcam_pose << cbcam_msg.pose.position.x, cbcam_msg.pose.position.y, cbcam_yaw; + cbcam_pose << cbcam_msg.pose.position.x, cbcam_msg.pose.position.y, this->lidar_yaw; // align timestamp of prediction and correction if(if_align){ @@ -255,7 +267,7 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ ekf.Vprediction(this->local_twist, this->local_cov, dt_align); } - ekf.correction(cbcam_pose, this->cbcam_cov); + ekf.correction(cbcam_pose, this->cbcam_h, this->cbcam_cov); Eigen::Vector3d robot_pose = ekf.getPose(); @@ -297,7 +309,7 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ double obcam_yaw = utils::qua2yaw(obcam_msg.pose.orientation); - obcam_pose << obcam_msg.pose.position.x, obcam_msg.pose.position.y, obcam_yaw; + obcam_pose << obcam_msg.pose.position.x, obcam_msg.pose.position.y, this->lidar_yaw; // align timestamp of prediction and correction if(if_align){ @@ -309,7 +321,7 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ ekf.Vprediction(this->local_twist, this->local_cov, dt_align); } - ekf.correction(obcam_pose, this->obcam_cov); + ekf.correction(obcam_pose, this->obcam_h, this->obcam_cov); Eigen::Vector3d robot_pose = ekf.getPose(); diff --git a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml index e6bb8fa..53f323c 100644 --- a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml +++ b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yml @@ -5,4 +5,4 @@ lidar_localization: visualize_candidate: true likelihood_threshold: 0.85 consistency_threshold: 0.92 - lidar_multiplier: 0.987 \ No newline at end of file + lidar_multiplier: 0.993 \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py index 37019fa..94e7f1b 100644 --- a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py +++ b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py @@ -27,7 +27,7 @@ def __init__(self): self.declare_parameter('consistency_threshold', 0.93) self.declare_parameter('robot_frame_id', 'base_footprint') self.declare_parameter('robot_parent_frame_id', 'map') - self.declare_parameter('lidar_multiplier', 0.987) + self.declare_parameter('lidar_multiplier', 0.993) self.declare_parameter('likelihood_threshold_two', 0.95) self.declare_parameter('consistency_threshold_two', 0.99) @@ -276,8 +276,8 @@ def get_obs_candidate(self, landmark, obs_raw): di_square = y.T @ S_inv @ y likelihood = np.exp(-0.5 * di_square) if likelihood > self.likelihood_threshold: - obs[0] = 0.987*obs[0] - obs[1] = 0.987*obs[1] + obs[0] = self.lidar_multiplier*obs[0] + obs[1] = self.lidar_multiplier*obs[1] if likelihood > self.likelihood_threshold: obs_candidates.append({'position': obs, 'probability': likelihood}) else: From e71cc066c3ee5222460177b0b05b903a45627f12 Mon Sep 17 00:00:00 2001 From: robert Date: Wed, 4 Mar 2026 16:10:23 +0800 Subject: [PATCH 15/22] Style: change variables that are not necessary to be global to local --- .../fusion/include/fusion/fusion_node.hpp | 3 --- .../core_algorithm/fusion/src/fusion_node.cpp | 22 +++++++++---------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp index 8058161..4da17d9 100644 --- a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp +++ b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp @@ -49,12 +49,9 @@ class FusionNode : public rclcpp::Node{ Eigen::Vector3d local_twist; Eigen::Vector3d init_state; - Eigen::Vector3d mapodom_state; Eigen::Matrix3d local_cov; Eigen::Matrix3d cbcam_cov; Eigen::Matrix3d obcam_cov; - Eigen::Matrix3d init_rot; - Eigen::Matrix3d mapodom_rot; Eigen::Matrix3d global_h; Eigen::Matrix3d cbcam_h; Eigen::Matrix3d obcam_h; diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index 8d9709d..068f3cb 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -17,7 +17,6 @@ FusionNode::FusionNode() : Node("fusion_node"){ this->local_cov.setIdentity(); this->cbcam_cov.setIdentity(); this->obcam_cov.setIdentity(); - this->init_rot.setIdentity(); this->global_h.setIdentity(); this->cbcam_h.setIdentity(); this->obcam_h.setIdentity(); @@ -117,10 +116,6 @@ void FusionNode::initCallback(const geometry_msgs::msg::PoseWithCovariance & ini this->init_state(1) = init_msg.pose.position.y; this->init_state(2) = utils::qua2yaw(init_msg.pose.orientation); - this->init_rot << cos(this->init_state(2)), -sin(this->init_state(2)), 0, - sin(this->init_state(2)), cos(this->init_state(2)), 0, - 0, 0, 1; - Eigen::Matrix3d init_cov; init_cov << init_msg.covariance[0], 0, 0, 0, init_msg.covariance[7], 0, @@ -163,15 +158,18 @@ void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform(this->global_frame, this->robot_parent_frame, tf2::TimePointZero); - this->mapodom_state(0) = tf.transform.translation.x; - this->mapodom_state(1) = tf.transform.translation.y; - this->mapodom_state(2) = utils::qua2yaw(tf.transform.rotation); + Eigen::Vector3d tf_vec; + + tf_vec(0) = tf.transform.translation.x; + tf_vec(1) = tf.transform.translation.y; + tf_vec(2) = utils::qua2yaw(tf.transform.rotation); - this->mapodom_rot << cos(this->mapodom_state(2)), -sin(this->mapodom_state(2)), 0, - sin(this->mapodom_state(2)), cos(this->mapodom_state(2)), 0, - 0, 0, 1; + Eigen::Matrix3d tf_rot; + tf_rot << cos(tf_vec(2)), -sin(tf_vec(2)), 0, + sin(tf_vec(2)), cos(tf_vec(2)), 0, + 0, 0, 1; - Eigen::Vector3d predicted_pose = this->mapodom_state + this->mapodom_rot*local_pose; + Eigen::Vector3d predicted_pose = tf_vec + tf_rot*local_pose; ekf.Xprediction(predicted_pose, this->local_cov); Eigen::Vector3d robot_pose = ekf.getPose(); From 00e484d38f20c0e3a544c482fb943f175ecdb07e Mon Sep 17 00:00:00 2001 From: robert Date: Wed, 4 Mar 2026 16:16:25 +0800 Subject: [PATCH 16/22] Docs: remove ros1bridge settings since we jave microROS now --- docker/.env | 2 +- docker/Dockerfile.communication | 75 --- docker/config/bridge.yaml | 9 - docker/docker-compose.yaml | 84 +-- docker/modules/install_ros_bridge.sh | 20 - docker/modules/install_ros_humble.sh | 51 -- .../firmware_comm/CMakeLists.txt | 30 - docker/sensor_dep_ros1/firmware_comm/Makefile | 1 - .../firmware_comm/launch/firmware_comm.launch | 13 - .../firmware_comm/mainpage.dox | 14 - .../firmware_comm/manifest.xml | 13 - .../rosserial_msgs/CHANGELOG.rst | 105 --- .../rosserial_msgs/CMakeLists.txt | 21 - .../rosserial_msgs/msg/Log.msg | 10 - .../rosserial_msgs/msg/TopicInfo.msg | 21 - .../rosserial_msgs/package.xml | 18 - .../rosserial_msgs/srv/RequestParam.srv | 7 - .../rosserial_server/CHANGELOG.rst | 130 ---- .../rosserial_server/CMakeLists.txt | 77 --- .../rosserial_server/async_read_buffer.h | 204 ------ .../include/rosserial_server/msg_lookup.h | 44 -- .../include/rosserial_server/serial_session.h | 118 ---- .../include/rosserial_server/session.h | 625 ------------------ .../include/rosserial_server/tcp_server.h | 92 --- .../include/rosserial_server/topic_handlers.h | 195 ------ .../rosserial_server/udp_socket_session.h | 94 --- .../include/rosserial_server/udp_stream.h | 133 ---- .../rosserial_server/launch/serial.launch | 6 - .../rosserial_server/launch/socket.launch | 3 - .../rosserial_server/launch/udp_socket.launch | 9 - .../rosserial_server/package.xml | 23 - .../rosserial_server/src/msg_lookup.cpp | 102 --- .../rosserial_server/src/serial_node.cpp | 56 -- .../rosserial_server/src/socket_node.cpp | 56 -- .../rosserial_server/src/udp_socket_node.cpp | 65 -- 35 files changed, 2 insertions(+), 2524 deletions(-) delete mode 100644 docker/Dockerfile.communication delete mode 100644 docker/config/bridge.yaml delete mode 100644 docker/modules/install_ros_bridge.sh delete mode 100644 docker/modules/install_ros_humble.sh delete mode 100644 docker/sensor_dep_ros1/firmware_comm/CMakeLists.txt delete mode 100644 docker/sensor_dep_ros1/firmware_comm/Makefile delete mode 100644 docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch delete mode 100644 docker/sensor_dep_ros1/firmware_comm/mainpage.dox delete mode 100644 docker/sensor_dep_ros1/firmware_comm/manifest.xml delete mode 100644 docker/sensor_dep_ros1/rosserial_msgs/CHANGELOG.rst delete mode 100644 docker/sensor_dep_ros1/rosserial_msgs/CMakeLists.txt delete mode 100644 docker/sensor_dep_ros1/rosserial_msgs/msg/Log.msg delete mode 100644 docker/sensor_dep_ros1/rosserial_msgs/msg/TopicInfo.msg delete mode 100644 docker/sensor_dep_ros1/rosserial_msgs/package.xml delete mode 100644 docker/sensor_dep_ros1/rosserial_msgs/srv/RequestParam.srv delete mode 100644 docker/sensor_dep_ros1/rosserial_server/CHANGELOG.rst delete mode 100644 docker/sensor_dep_ros1/rosserial_server/CMakeLists.txt delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/async_read_buffer.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/msg_lookup.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/serial_session.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/session.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/tcp_server.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/topic_handlers.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_socket_session.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_stream.h delete mode 100644 docker/sensor_dep_ros1/rosserial_server/launch/serial.launch delete mode 100644 docker/sensor_dep_ros1/rosserial_server/launch/socket.launch delete mode 100644 docker/sensor_dep_ros1/rosserial_server/launch/udp_socket.launch delete mode 100644 docker/sensor_dep_ros1/rosserial_server/package.xml delete mode 100644 docker/sensor_dep_ros1/rosserial_server/src/msg_lookup.cpp delete mode 100644 docker/sensor_dep_ros1/rosserial_server/src/serial_node.cpp delete mode 100644 docker/sensor_dep_ros1/rosserial_server/src/socket_node.cpp delete mode 100644 docker/sensor_dep_ros1/rosserial_server/src/udp_socket_node.cpp diff --git a/docker/.env b/docker/.env index 86d5e20..7acbc4a 100644 --- a/docker/.env +++ b/docker/.env @@ -6,7 +6,7 @@ COMPOSE_FILE=docker-compose.yaml ROS_DOMAIN_ID=13 ## Release version -COMPOSE_PROFILES = communication, onboard-running +COMPOSE_PROFILES = onboard-running USER_UID=1004 MODE=yellow diff --git a/docker/Dockerfile.communication b/docker/Dockerfile.communication deleted file mode 100644 index f291f25..0000000 --- a/docker/Dockerfile.communication +++ /dev/null @@ -1,75 +0,0 @@ -FROM ubuntu:22.04 AS base - -LABEL maintainer="Yu Hsiang Chen" -LABEL email="seanchen1117@gmail.com" - -ENV DEBIAN_FRONTEND=noninteractive - -# Keep apt cache to speed up subsequent builds -RUN rm -f /etc/apt/apt.conf.d/docker-clean; echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' > /etc/apt/apt.conf.d/keep-cache - -# Install common tools -RUN --mount=type=cache,target=/var/cache/apt,sharing=private \ - apt-get update && apt-get install -y \ - curl \ - git \ - sudo \ - usbutils \ - v4l-utils \ - htop \ - iputils-ping \ - wget \ - net-tools \ - tmux \ - vim \ - wget \ - && rm -rf /var/lib/apt/lists/* - -FROM base AS build - -ENV DEBIAN_FRONTEND=noninteractive - -RUN rm -f /etc/apt/apt.conf.d/docker-clean; echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' > /etc/apt/apt.conf.d/keep-cache - -# Install ROS 2 Humble -COPY modules/install_ros_humble.sh /tmp/install_ros_humble.sh -RUN --mount=type=cache,target=/var/cache/apt,sharing=private \ - /tmp/install_ros_humble.sh && rm /tmp/install_ros_humble.sh - -# Install ROS Noetic -# ref: https://docs.ros.org/en/humble/How-To-Guides/Using-ros1_bridge-Jammy-upstream.html -# ref: https://github.com/osrf/docker_images/issues/635#issuecomment-1217505552 -# If there is still issue, try to comment the command below and enter the container to see the correct file name under /etc/apt/sources.list.d/ -RUN rm -rf /etc/apt/sources.list.d/ros2.sources -RUN --mount=type=cache,target=/var/cache/apt,sharing=private \ - apt-get update \ - && apt remove -y python3-catkin-pkg python3-catkin-pkg-modules\ - && apt-get install -q -y --no-install-recommends ros-core-dev \ - && rm -rf /var/lib/apt/lists/* - -# Install ros1_bridge -WORKDIR /ros2_humble -COPY modules/install_ros_bridge.sh /tmp/install_ros_bridge.sh -RUN --mount=type=cache,target=/var/cache/apt,sharing=private \ - /tmp/install_ros_bridge.sh && rm /tmp/install_ros_bridge.sh - -ENTRYPOINT [] -CMD ["/bin/bash"] - -FROM base AS deploy - -ENV DEBIAN_FRONTEND=noninteractive - -RUN rm -f /etc/apt/apt.conf.d/docker-clean; echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' > /etc/apt/apt.conf.d/keep-cache - -RUN --mount=type=cache,target=/var/cache/apt,sharing=private \ - apt-get update && apt-get install -y \ - libspdlog-dev \ - python3-packaging \ - ros-core-dev - -RUN mkdir -p /ros2_humble -COPY --from=build /ros2_humble/install /ros2_humble/install - -ENTRYPOINT [ ] -CMD ["/bin/bash"] \ No newline at end of file diff --git a/docker/config/bridge.yaml b/docker/config/bridge.yaml deleted file mode 100644 index e2639ed..0000000 --- a/docker/config/bridge.yaml +++ /dev/null @@ -1,9 +0,0 @@ -topics: - - - topic: /wheel_pose - type: geometry_msgs/msg/Twist - queue_size: 1 - - - topic: /cmd_vel - type: geometry_msgs/msg/Twist - queue_size: 1 \ No newline at end of file diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 43ea53f..f757d31 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -63,86 +63,4 @@ services: cd /home/user/localization-ws && colcon build && source install/setup.bash && - ros2 launch localization_bringup localization.launch.py mode:=${MODE}" - - # containers for comunicating with firmware - ros-core: - profiles: ['communication'] - image: ros:noetic-ros-base - container_name: ros-core - privileged: true - network_mode: host - stdin_open: true - tty: true - working_dir: / - stop_grace_period: 1s - healthcheck: - # The healthcheck is required for ros1-bridge to wait until roscore is ready. - test: /ros_entrypoint.sh bash -c "rostopic list || exit 1" - interval: 3s - timeout: 10s - retries: 5 - volumes: - - /etc/timezone:/etc/timezone:ro - - /etc/localtime:/etc/localtime:ro - - /dev:/dev - command: bash -c "source /opt/ros/noetic/setup.bash - && roscore" - - ros1bridge: - profiles: ['communication'] - build: - context: . - dockerfile: Dockerfile.communication - target: deploy - image: cyhsiang/dit-ros1bridge:latest - container_name: ros1bridge - privileged: true - network_mode: host - stdin_open: true - tty: true - working_dir: / - restart: on-failure - depends_on: - ros-core: - # The healthcheck is required for ros1-bridge to wait until roscore is ready. - condition: service_healthy - environment: - - ROS_LOCALHOST_ONLY=0 - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} - - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - - volumes: - - /etc/timezone:/etc/timezone:ro - - /etc/localtime:/etc/localtime:ro - - /dev:/dev - command: bash -c "source /ros2_humble/install/setup.bash - && source /ros2_humble/install/ros1_bridge/share/ros1_bridge/local_setup.bash - && export ROS_MASTER_URI=http://localhost:11311 - && ros2 run ros1_bridge parameter_bridge 2>/dev/null" - - ros1: - profiles: ['communication'] - image: osrf/ros:noetic-desktop-full - container_name: ros1 - stdin_open: true - tty: true - network_mode: host - privileged: true - depends_on: - ros-core: - condition: service_healthy - volumes: - - /etc/timezone:/etc/timezone:ro - - /etc/localtime:/etc/localtime:ro - - /dev:/dev - - ../docker/sensor_dep_ros1:/home/user/ros1-ws/src - - ../docker/config:/home/config - stop_grace_period: 1s - command: /bin/bash -c " - source /opt/ros/noetic/setup.bash && - rosparam load /home/config/bridge.yaml && - cd /home/user/ros1-ws && - catkin_make && - source devel/setup.bash && - roslaunch firmware_comm firmware_comm.launch" \ No newline at end of file + ros2 launch localization_bringup localization.launch.py mode:=${MODE}" \ No newline at end of file diff --git a/docker/modules/install_ros_bridge.sh b/docker/modules/install_ros_bridge.sh deleted file mode 100644 index 7e2f1b7..0000000 --- a/docker/modules/install_ros_bridge.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/bin/bash -set -e - -echo "Installing ROS Bridge..." - -apt update && apt install -y git - -# Create a workspace for the ros1_bridge -mkdir -p src/ros1_bridge/src \ - && cd src/ros1_bridge/src \ - && git clone https://github.com/ros2/ros1_bridge \ - && cd /ros2_humble - -# Source the ROS 2 workspace -source install/setup.bash - -# Build -colcon build --packages-select ros1_bridge --cmake-force-configure - -echo "ROS Bridge installation completed successfully!" \ No newline at end of file diff --git a/docker/modules/install_ros_humble.sh b/docker/modules/install_ros_humble.sh deleted file mode 100644 index 9985ca5..0000000 --- a/docker/modules/install_ros_humble.sh +++ /dev/null @@ -1,51 +0,0 @@ -#!/bin/bash -set -e - -echo "Installing ROS Humble from source..." - -# Ref: https://docs.ros.org/en/humble/Installation/Alternatives/Ubuntu-Development-Setup.html -# Set locale -apt update && apt install -y locales -locale-gen en_US en_US.UTF-8 -update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 -export LANG=en_US.UTF-8 - -# Setup sources -apt install -y software-properties-common -add-apt-repository universe - -apt update && apt install curl -y -export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') -curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" -dpkg -i /tmp/ros2-apt-source.deb - -# Install development tools and ROS tools -apt update && apt install -y \ - python3-flake8-docstrings \ - python3-pip \ - python3-pytest-cov \ - ros-dev-tools \ - python3-flake8-blind-except \ - python3-flake8-builtins \ - python3-flake8-class-newline \ - python3-flake8-comprehensions \ - python3-flake8-deprecated \ - python3-flake8-import-order \ - python3-flake8-quotes \ - python3-pytest-repeat \ - python3-pytest-rerunfailures - -python3 -m pip install -U colcon-common-extensions vcstool - -mkdir -p /ros2_humble/src \ - && cd /ros2_humble \ - && vcs import --input https://raw.githubusercontent.com/ros2/ros2/humble/ros2.repos src - -apt update && apt upgrade -y - -rosdep init \ - && rosdep update \ - && rosdep install --from-paths src --ignore-src -y --skip-keys="fastcdr rti-connext-dds-6.0.1 urdfdom_headers" - -colcon build -echo "ROS Humble installation completed successfully!" \ No newline at end of file diff --git a/docker/sensor_dep_ros1/firmware_comm/CMakeLists.txt b/docker/sensor_dep_ros1/firmware_comm/CMakeLists.txt deleted file mode 100644 index f8f1c9c..0000000 --- a/docker/sensor_dep_ros1/firmware_comm/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -rosbuild_init() - -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) diff --git a/docker/sensor_dep_ros1/firmware_comm/Makefile b/docker/sensor_dep_ros1/firmware_comm/Makefile deleted file mode 100644 index b75b928..0000000 --- a/docker/sensor_dep_ros1/firmware_comm/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch b/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch deleted file mode 100644 index 3cd3a7f..0000000 --- a/docker/sensor_dep_ros1/firmware_comm/launch/firmware_comm.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - require: - publishers: [ wheel_pose ] - subscribers: [ cmd_vel ] - - - - \ No newline at end of file diff --git a/docker/sensor_dep_ros1/firmware_comm/mainpage.dox b/docker/sensor_dep_ros1/firmware_comm/mainpage.dox deleted file mode 100644 index d69c247..0000000 --- a/docker/sensor_dep_ros1/firmware_comm/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b firmware_comm - - - ---> - - -*/ diff --git a/docker/sensor_dep_ros1/firmware_comm/manifest.xml b/docker/sensor_dep_ros1/firmware_comm/manifest.xml deleted file mode 100644 index a8cd6e2..0000000 --- a/docker/sensor_dep_ros1/firmware_comm/manifest.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - firmware_comm - - - root - BSD - - http://ros.org/wiki/firmware_comm - - - diff --git a/docker/sensor_dep_ros1/rosserial_msgs/CHANGELOG.rst b/docker/sensor_dep_ros1/rosserial_msgs/CHANGELOG.rst deleted file mode 100644 index ee8c98d..0000000 --- a/docker/sensor_dep_ros1/rosserial_msgs/CHANGELOG.rst +++ /dev/null @@ -1,105 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rosserial_msgs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.9.2 (2021-04-02) ------------------- - -0.9.1 (2020-09-09) ------------------- - -0.9.0 (2020-08-25) ------------------- -* Fix Travis for Noetic + Python 3 -* Bump minimum CMake version to 3.7.2 (Melodic). -* Drop separate node for message service (`#446 `_) -* Contributors: Mike Purvis - -0.8.0 (2018-10-11) ------------------- - -0.7.7 (2017-11-29) ------------------- -* Fix catkin lint errors (`#296 `_) -* Contributors: Bei Chen Liu - -0.7.6 (2017-03-01) ------------------- - -0.7.5 (2016-11-22) ------------------- - -0.7.4 (2016-09-21) ------------------- - -0.7.3 (2016-08-05) ------------------- - -0.7.2 (2016-07-15) ------------------- - -0.7.1 (2015-07-06) ------------------- - -0.7.0 (2015-04-23) ------------------- - -0.6.3 (2014-11-05) ------------------- - -0.6.2 (2014-09-10) ------------------- - -0.6.1 (2014-06-30) ------------------- - -0.6.0 (2014-06-11) ------------------- -* Uncomment ID_TX_STOP constant, per `#111 `_ -* Contributors: Mike Purvis - -0.5.6 (2014-06-11) ------------------- -* Add Mike Purvis as maintainer to all but xbee. -* remove ID_TX_STOP from rosserial_msgs/msg/TopicInfo.msg, using hardcode modification. fix the dupilcated registration problem of subscriber -* modified rosserial -* Contributors: Mike Purvis, Moju Zhao - -0.5.5 (2014-01-14) ------------------- - -0.5.4 (2013-10-17) ------------------- - -0.5.3 (2013-09-21) ------------------- -* Add message info service and stub of node to provide it, for -rosserial_server support. - -0.5.2 (2013-07-17) ------------------- - -* Fix release version - -0.5.1 (2013-07-15) ------------------- - -0.4.5 (2013-07-02) ------------------- - -0.4.4 (2013-03-20) ------------------- - -0.4.3 (2013-03-13 14:08) ------------------------- - -0.4.2 (2013-03-13 01:15) ------------------------- - -0.4.1 (2013-03-09) ------------------- - -0.4.0 (2013-03-08) ------------------- -* Changed DEBUG log level to ROSDEBUG. -* initial catkin version on github diff --git a/docker/sensor_dep_ros1/rosserial_msgs/CMakeLists.txt b/docker/sensor_dep_ros1/rosserial_msgs/CMakeLists.txt deleted file mode 100644 index c525612..0000000 --- a/docker/sensor_dep_ros1/rosserial_msgs/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 3.7.2) -project(rosserial_msgs) - -find_package(catkin REQUIRED COMPONENTS - message_generation -) - -add_message_files(FILES - Log.msg - TopicInfo.msg -) - -add_service_files(FILES - RequestParam.srv -) - -generate_messages() - -catkin_package(CATKIN_DEPENDS - message_runtime -) diff --git a/docker/sensor_dep_ros1/rosserial_msgs/msg/Log.msg b/docker/sensor_dep_ros1/rosserial_msgs/msg/Log.msg deleted file mode 100644 index 7239876..0000000 --- a/docker/sensor_dep_ros1/rosserial_msgs/msg/Log.msg +++ /dev/null @@ -1,10 +0,0 @@ - -#ROS Logging Levels -uint8 ROSDEBUG=0 -uint8 INFO=1 -uint8 WARN=2 -uint8 ERROR=3 -uint8 FATAL=4 - -uint8 level -string msg diff --git a/docker/sensor_dep_ros1/rosserial_msgs/msg/TopicInfo.msg b/docker/sensor_dep_ros1/rosserial_msgs/msg/TopicInfo.msg deleted file mode 100644 index dafd6b0..0000000 --- a/docker/sensor_dep_ros1/rosserial_msgs/msg/TopicInfo.msg +++ /dev/null @@ -1,21 +0,0 @@ -# special topic_ids -uint16 ID_PUBLISHER=0 -uint16 ID_SUBSCRIBER=1 -uint16 ID_SERVICE_SERVER=2 -uint16 ID_SERVICE_CLIENT=4 -uint16 ID_PARAMETER_REQUEST=6 -uint16 ID_LOG=7 -uint16 ID_TIME=10 -uint16 ID_TX_STOP=11 - -# The endpoint ID for this topic -uint16 topic_id - -string topic_name -string message_type - -# MD5 checksum for this message type -string md5sum - -# size of the buffer message must fit in -int32 buffer_size diff --git a/docker/sensor_dep_ros1/rosserial_msgs/package.xml b/docker/sensor_dep_ros1/rosserial_msgs/package.xml deleted file mode 100644 index 9f8ac6d..0000000 --- a/docker/sensor_dep_ros1/rosserial_msgs/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - rosserial_msgs - 0.9.2 - - Messages for automatic topic configuration using rosserial. - - Michael Ferguson - Paul Bouchier - Mike Purvis - BSD - http://ros.org/wiki/rosserial_msgs - - catkin - - message_generation - - message_runtime - diff --git a/docker/sensor_dep_ros1/rosserial_msgs/srv/RequestParam.srv b/docker/sensor_dep_ros1/rosserial_msgs/srv/RequestParam.srv deleted file mode 100644 index ca605e8..0000000 --- a/docker/sensor_dep_ros1/rosserial_msgs/srv/RequestParam.srv +++ /dev/null @@ -1,7 +0,0 @@ -string name - ---- - -int32[] ints -float32[] floats -string[] strings diff --git a/docker/sensor_dep_ros1/rosserial_server/CHANGELOG.rst b/docker/sensor_dep_ros1/rosserial_server/CHANGELOG.rst deleted file mode 100644 index e8d8efd..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/CHANGELOG.rst +++ /dev/null @@ -1,130 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rosserial_server -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.9.2 (2021-04-02) ------------------- -* Add python3-dev as build depend (`#544 `_) -* Contributors: Tobias Fischer - -0.9.1 (2020-09-09) ------------------- -* Add boost::thread dependency to rosserial_server (`#513 `_) -* Contributors: Mike Purvis - -0.9.0 (2020-08-25) ------------------- -* Only initialize embedded python interpreter once. (`#491 `_) -* Port 482 and 483 forward from Melodic branch (`#492 `_) -* Fix warning when using std_msgs/Empty (`#482 `_) -* Bump minimum CMake version to 3.7.2 (Melodic). -* Removed unused service client for message info service (`#481 `_) -* Call io_service.stop() when ros::ok() returns false (`#477 `_) -* Call Py_Finalize before throwing exception (`#476 `_) -* [Windows] use c++ signed trait to replace ssize_t for better portability. (`#463 `_) -* Port rosserial_server to Boost 1.71. (`#468 `_) -* rosserial_server: update install rules for binary targets (`#457 `_) -* Fix bug: assign the md5 for service (`#449 `_) -* Contributors: Hermann von Kleist, Johannes Meyer, Mike Purvis, Sean Yen, 趙　漠居(Zhao, Moju) - -0.8.0 (2018-10-11) ------------------- -* Fix compiling on boost > 1.66 (`#362 `_) - Reflective of changes made to boost::asio noted here: - http://www.boost.org/doc/libs/1_66_0/doc/html/boost_asio/net_ts.html -* Contributors: Fan Jiang - -0.7.7 (2017-11-29) ------------------- -* Fix catkin lint errors (`#296 `_) -* Contributors: Bei Chen Liu - -0.7.6 (2017-03-01) ------------------- - -0.7.5 (2016-11-22) ------------------- -* Fixing build errors for boost >=1.60 (`#226 `_) (`#250 `_) -* Contributors: Malte Splietker - -0.7.4 (2016-09-21) ------------------- -* Use catkin_EXPORTED_TARGETS to avoid CMake warning (`#246 `_) -* Fix AsyncReadBuffer for UDP socket case. (`#245 `_) -* Contributors: Mike Purvis - -0.7.3 (2016-08-05) ------------------- -* Avoid runaway async condition when port is bad. (`#236 `_) -* Add missing install rule for udp_socket_node -* Make the ~require param configurable from Session. (`#233 `_) -* Contributors: Mike Purvis - -0.7.2 (2016-07-15) ------------------- -* Implementation of native UDP rosserial server. (`#231 `_) -* Explicit session lifecycle for the serial server. (`#228 `_) - This is a long overdue change which will resolve some crashes when - USB serial devices return error states in the face of noise or other - interruptions. -* Support for VER1 protocol has been dropped. -* Handle log messages in rosserial_server -* Contributors: Mike Purvis, mkrauter - -0.7.1 (2015-07-06) ------------------- - -0.7.0 (2015-04-23) ------------------- -* Fill out description field in package.xml. -* Bugfix for checksum. - Publishing topics fails when messages are over 256 bytes in length due to checksum() function or'ing high and low byte instead of adding them. -* rosserial_server: Properly receive messages > 255 bytes. -* Contributors: Chad Attermann, Mike Purvis - -0.6.3 (2014-11-05) ------------------- -* Add more log output, don't end the session for certain write errors. -* Contributors: Mike Purvis - -0.6.2 (2014-09-10) ------------------- -* Bugfix for interrupted sessions. - This is a two-part fix for an issue causes a segfault when the device - disappears during operation, for example a ttyACM device which is unplugged. - The AsyncReadBuffer part avoids calling a callback after the object - owning it has destructed, and the SerialSession part avoids recreating - itself until the previous instance has finished the destructor and been - full destroyed. -* Add dependency on rosserial_msgs_gencpp, fixes `#133 `_ -* Make ServiceClient::handle public, to fix compilation bug on some platforms. -* Enabled registration of service clients -* Add namespaces to headers, swap ROS thread to foreground. -* Move headers to include path, rename to follow ROS style. - -0.6.1 (2014-06-30) ------------------- - -0.6.0 (2014-06-11) ------------------- - -0.5.6 (2014-06-11) ------------------- -* Fixed build error due to variable being read as a function due to missing parenthesis -* Add rosserial_python as dependency of rosserial_server -* Contributors: Mike Purvis, spaghetti- - -0.5.5 (2014-01-14) ------------------- -* Add support for require/publishers and require/subscribers parameters. -* Use stream logging in rosserial_server - -0.5.4 (2013-10-17) ------------------- - -0.5.3 (2013-09-21) ------------------- -* New package: rosserial_server -* Contains example launch file for serial configuration of server -* Working now with both Groovy and Hydro clients. -* Subscriber to correctly declare known md5 and topic type from client. diff --git a/docker/sensor_dep_ros1/rosserial_server/CMakeLists.txt b/docker/sensor_dep_ros1/rosserial_server/CMakeLists.txt deleted file mode 100644 index 6e57211..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.7.2) -project(rosserial_server) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rosserial_msgs - std_msgs - topic_tools -) - -find_package(Boost REQUIRED COMPONENTS - system - thread -) - -find_package(PythonLibs REQUIRED) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - rosserial_msgs - std_msgs - topic_tools - LIBRARIES ${PROJECT_NAME}_lookup -) - -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME}_lookup src/msg_lookup.cpp) -target_link_libraries(${PROJECT_NAME}_lookup ${PYTHON_LIBRARY}) -target_include_directories(${PROJECT_NAME}_lookup SYSTEM PRIVATE ${PYTHON_INCLUDE_DIR}) - -add_executable(${PROJECT_NAME}_serial_node src/serial_node.cpp) -target_link_libraries(${PROJECT_NAME}_serial_node ${catkin_LIBRARIES} ${PROJECT_NAME}_lookup) -set_target_properties(${PROJECT_NAME}_serial_node PROPERTIES OUTPUT_NAME serial_node PREFIX "") -add_dependencies(${PROJECT_NAME}_serial_node ${catkin_EXPORTED_TARGETS}) - -add_executable(${PROJECT_NAME}_socket_node src/socket_node.cpp) -target_link_libraries(${PROJECT_NAME}_socket_node ${catkin_LIBRARIES} ${PROJECT_NAME}_lookup) -set_target_properties(${PROJECT_NAME}_socket_node PROPERTIES OUTPUT_NAME socket_node PREFIX "") -add_dependencies(${PROJECT_NAME}_socket_node ${catkin_EXPORTED_TARGETS}) - -add_executable(${PROJECT_NAME}_udp_socket_node src/udp_socket_node.cpp) -target_link_libraries(${PROJECT_NAME}_udp_socket_node ${catkin_LIBRARIES} ${PROJECT_NAME}_lookup) -set_target_properties(${PROJECT_NAME}_udp_socket_node PROPERTIES OUTPUT_NAME udp_socket_node PREFIX "") -add_dependencies(${PROJECT_NAME}_udp_socket_node ${catkin_EXPORTED_TARGETS}) - -install( - TARGETS - ${PROJECT_NAME}_lookup - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install( - TARGETS - ${PROJECT_NAME}_serial_node - ${PROJECT_NAME}_socket_node - ${PROJECT_NAME}_udp_socket_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install( - DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/async_read_buffer.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/async_read_buffer.h deleted file mode 100644 index 586fcae..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/async_read_buffer.h +++ /dev/null @@ -1,204 +0,0 @@ -/** - * - * \file - * \brief Helper object for successive reads from a ReadStream. - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_ASYNC_READ_BUFFER_H -#define ROSSERIAL_SERVER_ASYNC_READ_BUFFER_H - -#include -#include -#include - -#include - -// ssize_t is POSIX-only type. Use make_signed for portable code. -#include // size_t -#include // std::make_signed -typedef std::make_signed::type signed_size_t; - -namespace rosserial_server -{ - -template -class AsyncReadBuffer -{ -public: - AsyncReadBuffer(AsyncReadStream& s, size_t capacity, - boost::function error_callback) - : stream_(s), read_requested_bytes_(0), error_callback_(error_callback) { - reset(); - mem_.resize(capacity); - ROS_ASSERT_MSG(error_callback_, "Bad error callback passed to read buffer."); - } - - /** - * @brief Commands a fixed number of bytes from the buffer. This may be fulfilled from existing - * buffer content, or following a hardware read if required. - */ - void read(size_t requested_bytes, boost::function callback) { - ROS_DEBUG_STREAM_NAMED("async_read", "Buffer read of " << requested_bytes << " bytes, " << - "wi: " << write_index_ << ", ri: " << read_index_); - - ROS_ASSERT_MSG(read_requested_bytes_ == 0, "Bytes requested is nonzero, is there an operation already pending?"); - ROS_ASSERT_MSG(callback, "Bad read success callback function."); - read_success_callback_ = callback; - read_requested_bytes_ = requested_bytes; - - if (read_requested_bytes_ > mem_.size()) - { - // Insufficient room in the buffer for the requested bytes, - ROS_ERROR_STREAM_NAMED("async_read", "Requested to read " << read_requested_bytes_ << - " bytes, but buffer capacity is only " << mem_.size() << "."); - error_callback_(boost::system::errc::make_error_code(boost::system::errc::no_buffer_space)); - return; - } - - // Number of bytes which must be transferred to satisfy the request. - signed_size_t transfer_bytes = read_requested_bytes_ - bytesAvailable(); - - if (transfer_bytes > 0) - { - // If we don't have enough headroom in the buffer, we'll have to shift what's currently in there to make room. - if (bytesHeadroom() < transfer_bytes) - { - memmove(&mem_[0], &mem_[read_index_], bytesAvailable()); - write_index_ = bytesAvailable(); - read_index_ = 0; - } - - // Initiate a read from hardware so that we have enough bytes to fill the user request. - ROS_DEBUG_STREAM_NAMED("async_read", "Requesting transfer of at least " << transfer_bytes << " byte(s)."); - boost::asio::async_read(stream_, - boost::asio::buffer(&mem_[write_index_], bytesHeadroom()), - boost::asio::transfer_at_least(transfer_bytes), - boost::bind(&AsyncReadBuffer::callback, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); - } - else - { - // We have enough in the buffer already, can fill the request without going to hardware. - callSuccessCallback(); - } - } - -private: - void reset() - { - read_index_ = 0; - write_index_ = 0; - } - - inline size_t bytesAvailable() - { - return write_index_ - read_index_; - } - - inline size_t bytesHeadroom() - { - return mem_.size() - write_index_; - } - - /** - * @brief The internal callback which is called by the boost::asio::async_read invocation - * in the public read method above. - */ - void callback(const boost::system::error_code& error, size_t bytes_transferred) - { - if (error) - { - read_requested_bytes_ = 0; - read_success_callback_.clear(); - ROS_DEBUG_STREAM_NAMED("async_read", "Read operation failed with: " << error); - - if (error == boost::asio::error::operation_aborted) - { - // Special case for operation_aborted. The abort callback comes when the owning Session - // is in the middle of teardown, which means the callback is no longer valid. - } - else - { - error_callback_(error); - } - return; - } - - write_index_ += bytes_transferred; - ROS_DEBUG_STREAM_NAMED("async_read", "Successfully read " << bytes_transferred << " byte(s), now " << bytesAvailable() << " available."); - callSuccessCallback(); - } - - /** - * @brief Calls the user's callback. This is a separate function because it gets called from two - * places, depending whether or not an actual HW read is required to fill the request. - */ - void callSuccessCallback() - { - ROS_DEBUG_STREAM_NAMED("async_read", "Invoking success callback with buffer of requested size " << - read_requested_bytes_ << " byte(s)."); - - ros::serialization::IStream stream(&mem_[read_index_], read_requested_bytes_); - read_index_ += read_requested_bytes_; - - // Post the callback rather than executing it here so, so that we have a chance to do the cleanup - // below prior to it actually getting run, in the event that the callback queues up another read. -#if BOOST_VERSION >= 107000 - boost::asio::post(stream_.get_executor(), boost::bind(read_success_callback_, stream)); -#else - stream_.get_io_service().post(boost::bind(read_success_callback_, stream)); -#endif - - // Resetting these values clears our state so that we know there isn't a callback pending. - read_requested_bytes_ = 0; - read_success_callback_.clear(); - - if (bytesAvailable() == 0) - { - ROS_DEBUG_STREAM_NAMED("async_read", "Buffer is empty, resetting indexes to the beginning."); - reset(); - } - } - - AsyncReadStream& stream_; - std::vector mem_; - - size_t write_index_; - size_t read_index_; - boost::function error_callback_; - - boost::function read_success_callback_; - size_t read_requested_bytes_; -}; - -} // namespace - -#endif // ROSSERIAL_SERVER_ASYNC_READ_BUFFER_H diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/msg_lookup.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/msg_lookup.h deleted file mode 100644 index 28e60b9..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/msg_lookup.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * \author Mike Purvis - * \copyright Copyright (c) 2019, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - */ - -#include -#include - -namespace rosserial_server -{ - -struct MsgInfo -{ - std::string md5sum; - std::string full_text; -}; - -const MsgInfo lookupMessage(const std::string& message_type, const std::string submodule = "msg"); - -} // namespace rosserial_server diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/serial_session.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/serial_session.h deleted file mode 100644 index 2a0bc15..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/serial_session.h +++ /dev/null @@ -1,118 +0,0 @@ -/** - * - * \file - * \brief Single, reconnecting class for a serial rosserial session. - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_SERIAL_SESSION_H -#define ROSSERIAL_SERVER_SERIAL_SESSION_H - -#include -#include -#include - -#include - -#include "rosserial_server/session.h" - -namespace rosserial_server -{ - -class SerialSession : public Session -{ -public: - SerialSession(boost::asio::io_service& io_service, std::string port, int baud) - : Session(io_service), port_(port), baud_(baud), timer_(io_service) - { - ROS_INFO_STREAM("rosserial_server session configured for " << port_ << " at " << baud << "bps."); - - failed_connection_attempts_ = 0; - check_connection(); - } - -private: - void check_connection() - { - if (!is_active()) - { - attempt_connection(); - } - - // Every second, check again if the connection should be reinitialized, - // if the ROS node is still up. - if (ros::ok()) - { - timer_.expires_from_now(boost::posix_time::milliseconds(2000)); - timer_.async_wait(boost::bind(&SerialSession::check_connection, this)); - } - else - { - shutdown(); - } - } - - void attempt_connection() - { - ROS_DEBUG("Opening serial port."); - - boost::system::error_code ec; - socket().open(port_, ec); - if (ec) { - failed_connection_attempts_++; - if (failed_connection_attempts_ == 1) { - ROS_ERROR_STREAM("Unable to open port " << port_ << ": " << ec); - } else { - ROS_DEBUG_STREAM("Unable to open port " << port_ << " (" << failed_connection_attempts_ << "): " << ec); - } - return; - } - ROS_INFO_STREAM("Opened " << port_); - failed_connection_attempts_ = 0; - - typedef boost::asio::serial_port_base serial; - socket().set_option(serial::baud_rate(baud_)); - socket().set_option(serial::character_size(8)); - socket().set_option(serial::stop_bits(serial::stop_bits::one)); - socket().set_option(serial::parity(serial::parity::none)); - socket().set_option(serial::flow_control(serial::flow_control::none)); - - // Kick off the session. - start(); - } - - std::string port_; - int baud_; - boost::asio::deadline_timer timer_; - int failed_connection_attempts_; -}; - -} // namespace - -#endif // ROSSERIAL_SERVER_SERIAL_SESSION_H diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/session.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/session.h deleted file mode 100644 index e7f8ab7..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/session.h +++ /dev/null @@ -1,625 +0,0 @@ -/** - * - * \file - * \brief Class representing a session between this node and a - * templated rosserial client. - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_SESSION_H -#define ROSSERIAL_SERVER_SESSION_H - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "rosserial_server/async_read_buffer.h" -#include "rosserial_server/topic_handlers.h" - -namespace rosserial_server -{ - - typedef std::vector Buffer; - typedef boost::shared_ptr BufferPtr; - - template - class Session : boost::noncopyable - { - public: - Session(boost::asio::io_service &io_service) - : io_service_(io_service), - socket_(io_service), - sync_timer_(io_service), - require_check_timer_(io_service), - ros_spin_timer_(io_service), - async_read_buffer_(socket_, buffer_max, - boost::bind(&Session::read_failed, this, - boost::asio::placeholders::error)) - { - active_ = false; - - timeout_interval_ = boost::posix_time::milliseconds(5000); - attempt_interval_ = boost::posix_time::milliseconds(1000); - require_check_interval_ = boost::posix_time::milliseconds(1000); - ros_spin_interval_ = boost::posix_time::milliseconds(10); - require_param_name_ = "~require"; - - unrecognised_topic_retry_threshold_ = ros::param::param("~unrecognised_topic_retry_threshold", 0); - - nh_.setCallbackQueue(&ros_callback_queue_); - - // Intermittent callback to service ROS callbacks. To avoid polling like this, - // CallbackQueue could in the future be extended with a scheme to monitor for - // callbacks on another thread, and then queue them up to be executed on this one. - ros_spin_timer_.expires_from_now(ros_spin_interval_); - ros_spin_timer_.async_wait(boost::bind(&Session::ros_spin_timeout, this, - boost::asio::placeholders::error)); - } - - Socket &socket() - { - return socket_; - } - - void start() - { - ROS_DEBUG("Starting session."); - - callbacks_[rosserial_msgs::TopicInfo::ID_PUBLISHER] = boost::bind(&Session::setup_publisher, this, _1); - callbacks_[rosserial_msgs::TopicInfo::ID_SUBSCRIBER] = boost::bind(&Session::setup_subscriber, this, _1); - callbacks_[rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER] = boost::bind(&Session::setup_service_client_publisher, this, _1); - callbacks_[rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER] = boost::bind(&Session::setup_service_client_subscriber, this, _1); - callbacks_[rosserial_msgs::TopicInfo::ID_LOG] = boost::bind(&Session::handle_log, this, _1); - callbacks_[rosserial_msgs::TopicInfo::ID_TIME] = boost::bind(&Session::handle_time, this, _1); - - active_ = true; - attempt_sync(); - read_sync_header(); - } - - void stop() - { - std::cout << "Sending Tx Stop Request..." << std::endl; - uint8_t overhead_bytes = 8; - uint16_t length = overhead_bytes; - BufferPtr buffer_ptr(new Buffer(length)); - ros::serialization::OStream stream(&buffer_ptr->at(0), buffer_ptr->size()); - stream << (uint16_t)0xfeff << (uint16_t)0x0000 << (uint16_t)0x0bff << (uint16_t)0xf400; - boost::asio::async_write(socket_, boost::asio::buffer(*buffer_ptr), - boost::bind(&Session::write_completion_cb, this, boost::asio::placeholders::error, buffer_ptr)); - std::cout << "Stop..." << std::endl; - - // Abort any pending ROS callbacks. - ros_callback_queue_.clear(); - - // Abort active session timer callbacks, if present. - sync_timer_.cancel(); - require_check_timer_.cancel(); - - // Reset the state of the session, dropping any publishers or subscribers - // we currently know about from this client. - callbacks_.clear(); - subscribers_.clear(); - publishers_.clear(); - services_.clear(); - - // Close the socket. - socket_.close(); - active_ = false; - } - - void shutdown() - { - if (is_active()) - { - stop(); - } - io_service_.stop(); - } - - bool is_active() - { - return active_; - } - - /** - * This is to set the name of the required topics parameter from the - * default of ~require. You might want to do this to avoid a conflict - * with something else in that namespace, or because you're embedding - * multiple instances of rosserial_server in a single process. - */ - void set_require_param(std::string param_name) - { - require_param_name_ = param_name; - } - - private: - /** - * Periodic function which handles calling ROS callbacks, executed on the same - * io_service thread to avoid a concurrency nightmare. - */ - void ros_spin_timeout(const boost::system::error_code &error) - { - ros_callback_queue_.callAvailable(); - - if (ros::ok()) - { - // Call again next interval. - ros_spin_timer_.expires_from_now(ros_spin_interval_); - ros_spin_timer_.async_wait(boost::bind(&Session::ros_spin_timeout, this, - boost::asio::placeholders::error)); - } - else - { - shutdown(); - } - } - - //// RECEIVING MESSAGES //// - // TODO: Total message timeout, implement primarily in ReadBuffer. - - void read_sync_header() - { - async_read_buffer_.read(1, boost::bind(&Session::read_sync_first, this, _1)); - } - - void read_sync_first(ros::serialization::IStream &stream) - { - uint8_t sync; - stream >> sync; - if (sync == 0xff) - { - async_read_buffer_.read(1, boost::bind(&Session::read_sync_second, this, _1)); - } - else - { - read_sync_header(); - } - } - - void read_sync_second(ros::serialization::IStream &stream) - { - uint8_t sync; - stream >> sync; - if (sync == 0xfe) - { - async_read_buffer_.read(5, boost::bind(&Session::read_id_length, this, _1)); - } - else - { - read_sync_header(); - } - } - - void read_id_length(ros::serialization::IStream &stream) - { - uint16_t topic_id, length; - uint8_t length_checksum; - - // Check header checksum byte for length field. - stream >> length >> length_checksum; - if (length_checksum + checksum(length) != 0xff) - { - uint8_t csl = checksum(length); - ROS_WARN("Bad message header length checksum. Dropping message from client. T%d L%d C%d %d", topic_id, length, length_checksum, csl); - read_sync_header(); - return; - } - else - { - stream >> topic_id; - } - ROS_DEBUG("Received message header with length %d and topic_id=%d", length, topic_id); - - // Read message length + checksum byte. - async_read_buffer_.read(length + 1, boost::bind(&Session::read_body, this, - _1, topic_id)); - } - - void read_body(ros::serialization::IStream &stream, uint16_t topic_id) - { - ROS_DEBUG("Received body of length %d for message on topic %d.", stream.getLength(), topic_id); - - ros::serialization::IStream checksum_stream(stream.getData(), stream.getLength()); - uint8_t msg_checksum = checksum(checksum_stream) + checksum(topic_id); - - if (msg_checksum != 0xff) - { - ROS_WARN("Rejecting message on topicId=%d, length=%d with bad checksum.", topic_id, stream.getLength()); - } - else - { - if (callbacks_.count(topic_id) == 1) - { - try - { - // stream includes the check sum byte. - ros::serialization::IStream msg_stream(stream.getData(), stream.getLength() - 1); - callbacks_[topic_id](msg_stream); - } - catch (ros::serialization::StreamOverrunException e) - { - if (topic_id < 100) - { - ROS_ERROR("Buffer overrun when attempting to parse setup message."); - ROS_ERROR_ONCE("Is this firmware from a pre-Groovy rosserial?"); - } - else - { - ROS_WARN("Buffer overrun when attempting to parse user message."); - } - } - } - else - { - ROS_WARN("Received message with unrecognized topicId (%d).", topic_id); - - if ((unrecognised_topic_retry_threshold_ > 0) && ++unrecognised_topics_ >= unrecognised_topic_retry_threshold_) - { - // The threshold for unrecognised topics has been exceeded. - // Attempt to request the topics from the client again - ROS_WARN("Unrecognised topic threshold exceeded. Requesting topics from client"); - attempt_sync(); - unrecognised_topics_ = 0; - } - } - } - - // Kickoff next message read. - read_sync_header(); - } - - void read_failed(const boost::system::error_code &error) - { - if (error == boost::system::errc::no_buffer_space) - { - // No worries. Begin syncing on a new message. - ROS_WARN("Overrun on receive buffer. Attempting to regain rx sync."); - read_sync_header(); - } - else if (error) - { - // When some other read error has occurred, stop the session, which destroys - // all known publishers and subscribers. - ROS_WARN_STREAM("Socket asio error, closing socket: " << error); - stop(); - } - } - - //// SENDING MESSAGES //// - - void write_message(Buffer &message, const uint16_t topic_id) - { - uint8_t overhead_bytes = 8; - uint16_t length = overhead_bytes + message.size(); - BufferPtr buffer_ptr(new Buffer(length)); - - uint8_t msg_checksum; - ros::serialization::IStream checksum_stream(message.size() > 0 ? &message[0] : NULL, message.size()); - - ros::serialization::OStream stream(&buffer_ptr->at(0), buffer_ptr->size()); - uint8_t msg_len_checksum = 255 - checksum(message.size()); - stream << (uint16_t)0xfeff << (uint16_t)message.size() << msg_len_checksum << topic_id; - msg_checksum = 255 - (checksum(checksum_stream) + checksum(topic_id)); - - memcpy(stream.advance(message.size()), &message[0], message.size()); - stream << msg_checksum; - - ROS_DEBUG_NAMED("async_write", "Sending buffer of %d bytes to client.", length); - boost::asio::async_write(socket_, boost::asio::buffer(*buffer_ptr), - boost::bind(&Session::write_completion_cb, this, boost::asio::placeholders::error, buffer_ptr)); - } - - void write_completion_cb(const boost::system::error_code &error, - BufferPtr buffer_ptr) - { - if (error) - { - if (error == boost::system::errc::io_error) - { - ROS_WARN_THROTTLE(1, "Socket write operation returned IO error."); - } - else if (error == boost::system::errc::no_such_device) - { - ROS_WARN_THROTTLE(1, "Socket write operation returned no device."); - } - else - { - ROS_WARN_STREAM_THROTTLE(1, "Unknown error returned during write operation: " << error); - } - stop(); - } - // Buffer is destructed when this function exits and buffer_ptr goes out of scope. - } - - //// SYNC WATCHDOG //// - void attempt_sync() - { - request_topics(); - set_sync_timeout(attempt_interval_); - } - - void set_sync_timeout(const boost::posix_time::time_duration &interval) - { - if (ros::ok()) - { - sync_timer_.cancel(); - sync_timer_.expires_from_now(interval); - sync_timer_.async_wait(boost::bind(&Session::sync_timeout, this, - boost::asio::placeholders::error)); - } - else - { - shutdown(); - } - } - - void sync_timeout(const boost::system::error_code &error) - { - if (error != boost::asio::error::operation_aborted) - { - ROS_DEBUG("Sync with device lost."); - stop(); - } - } - - //// HELPERS //// - void request_topics() - { - std::vector message(0); - ROS_DEBUG("Sending request topics message for VER2 protocol."); - write_message(message, rosserial_msgs::TopicInfo::ID_PUBLISHER); - - // Set timer for future point at which to verify the subscribers and publishers - // created by the client against the expected set given in the parameters. - require_check_timer_.expires_from_now(require_check_interval_); - require_check_timer_.async_wait(boost::bind(&Session::required_topics_check, this, - boost::asio::placeholders::error)); - } - - void required_topics_check(const boost::system::error_code &error) - { - if (error != boost::asio::error::operation_aborted) - { - if (ros::param::has(require_param_name_)) - { - if (!check_set(require_param_name_ + "/publishers", publishers_) || - !check_set(require_param_name_ + "/subscribers", subscribers_)) - { - ROS_WARN("Connected client failed to establish the publishers and subscribers dictated by require parameter. Re-requesting topics."); - request_topics(); - } - } - } - } - - template - bool check_set(std::string param_name, M map) - { - XmlRpc::XmlRpcValue param_list; - ros::param::get(param_name, param_list); - ROS_ASSERT(param_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - for (int i = 0; i < param_list.size(); ++i) - { - ROS_ASSERT(param_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); - std::string required_topic((std::string(param_list[i]))); - // Iterate through map of registered topics, to ensure that this one is present. - bool found = false; - for (typename M::iterator j = map.begin(); j != map.end(); ++j) - { - if (nh_.resolveName(j->second->get_topic()) == - nh_.resolveName(required_topic)) - { - found = true; - ROS_INFO_STREAM("Verified connection to topic " << required_topic << ", given in parameter " << param_name); - break; - } - } - if (!found) - { - ROS_WARN_STREAM("Missing connection to topic " << required_topic << ", required by parameter " << param_name); - return false; - } - } - return true; - } - - static uint8_t checksum(ros::serialization::IStream &stream) - { - uint8_t sum = 0; - for (uint16_t i = 0; i < stream.getLength(); ++i) - { - sum += stream.getData()[i]; - } - return sum; - } - - static uint8_t checksum(uint16_t val) - { - return (val >> 8) + val; - } - - //// RECEIVED MESSAGE HANDLERS //// - - void setup_publisher(ros::serialization::IStream &stream) - { - rosserial_msgs::TopicInfo topic_info; - ros::serialization::Serializer::read(stream, topic_info); - - PublisherPtr pub(new Publisher(nh_, topic_info)); - callbacks_[topic_info.topic_id] = boost::bind(&Publisher::handle, pub, _1); - publishers_[topic_info.topic_id] = pub; - - set_sync_timeout(timeout_interval_); - } - - void setup_subscriber(ros::serialization::IStream &stream) - { - rosserial_msgs::TopicInfo topic_info; - ros::serialization::Serializer::read(stream, topic_info); - - SubscriberPtr sub(new Subscriber(nh_, topic_info, - boost::bind(&Session::write_message, this, _1, topic_info.topic_id))); - subscribers_[topic_info.topic_id] = sub; - - set_sync_timeout(timeout_interval_); - } - - // When the rosserial client creates a ServiceClient object (and/or when it registers that object with the NodeHandle) - // it creates a publisher (to publish the service request message to us) and a subscriber (to receive the response) - // the service client callback is attached to the *subscriber*, so when we receive the service response - // and wish to send it over the socket to the client, - // we must attach the topicId that came from the service client subscriber message - - void setup_service_client_publisher(ros::serialization::IStream &stream) - { - rosserial_msgs::TopicInfo topic_info; - ros::serialization::Serializer::read(stream, topic_info); - - if (!services_.count(topic_info.topic_name)) - { - ROS_DEBUG("Creating service client for topic %s", topic_info.topic_name.c_str()); - ServiceClientPtr srv(new ServiceClient( - nh_, topic_info, boost::bind(&Session::write_message, this, _1, _2))); - services_[topic_info.topic_name] = srv; - callbacks_[topic_info.topic_id] = boost::bind(&ServiceClient::handle, srv, _1); - } - if (services_[topic_info.topic_name]->getRequestMessageMD5() != topic_info.md5sum) - { - ROS_WARN("Service client setup: Request message MD5 mismatch between rosserial client and ROS"); - } - else - { - ROS_DEBUG("Service client %s: request message MD5 successfully validated as %s", - topic_info.topic_name.c_str(), topic_info.md5sum.c_str()); - } - set_sync_timeout(timeout_interval_); - } - - void setup_service_client_subscriber(ros::serialization::IStream &stream) - { - rosserial_msgs::TopicInfo topic_info; - ros::serialization::Serializer::read(stream, topic_info); - - if (!services_.count(topic_info.topic_name)) - { - ROS_DEBUG("Creating service client for topic %s", topic_info.topic_name.c_str()); - ServiceClientPtr srv(new ServiceClient( - nh_, topic_info, boost::bind(&Session::write_message, this, _1, _2))); - services_[topic_info.topic_name] = srv; - callbacks_[topic_info.topic_id] = boost::bind(&ServiceClient::handle, srv, _1); - } - // see above comment regarding the service client callback for why we set topic_id here - services_[topic_info.topic_name]->setTopicId(topic_info.topic_id); - if (services_[topic_info.topic_name]->getResponseMessageMD5() != topic_info.md5sum) - { - ROS_WARN("Service client setup: Response message MD5 mismatch between rosserial client and ROS"); - } - else - { - ROS_DEBUG("Service client %s: response message MD5 successfully validated as %s", - topic_info.topic_name.c_str(), topic_info.md5sum.c_str()); - } - set_sync_timeout(timeout_interval_); - } - - void handle_log(ros::serialization::IStream &stream) - { - rosserial_msgs::Log l; - ros::serialization::Serializer::read(stream, l); - if (l.level == rosserial_msgs::Log::ROSDEBUG) - ROS_DEBUG("%s", l.msg.c_str()); - else if (l.level == rosserial_msgs::Log::INFO) - ROS_INFO("%s", l.msg.c_str()); - else if (l.level == rosserial_msgs::Log::WARN) - ROS_WARN("%s", l.msg.c_str()); - else if (l.level == rosserial_msgs::Log::ERROR) - ROS_ERROR("%s", l.msg.c_str()); - else if (l.level == rosserial_msgs::Log::FATAL) - ROS_FATAL("%s", l.msg.c_str()); - } - - void handle_time(ros::serialization::IStream &stream) - { - std_msgs::Time time; - time.data = ros::Time::now(); - - size_t length = ros::serialization::serializationLength(time); - std::vector message(length); - - ros::serialization::OStream ostream(&message[0], length); - ros::serialization::Serializer::write(ostream, time); - - write_message(message, rosserial_msgs::TopicInfo::ID_TIME); - - // The MCU requesting the time from the server is the sync notification. This - // call moves the timeout forward. - set_sync_timeout(timeout_interval_); - } - - boost::asio::io_service &io_service_; - Socket socket_; - AsyncReadBuffer async_read_buffer_; - enum - { - buffer_max = 1023 - }; - bool active_; - - ros::NodeHandle nh_; - ros::CallbackQueue ros_callback_queue_; - - boost::posix_time::time_duration timeout_interval_; - boost::posix_time::time_duration attempt_interval_; - boost::posix_time::time_duration require_check_interval_; - boost::posix_time::time_duration ros_spin_interval_; - boost::asio::deadline_timer sync_timer_; - boost::asio::deadline_timer require_check_timer_; - boost::asio::deadline_timer ros_spin_timer_; - std::string require_param_name_; - int unrecognised_topic_retry_threshold_{0}; - int unrecognised_topics_{0}; - - std::map> callbacks_; - std::map publishers_; - std::map subscribers_; - std::map services_; - }; - -} // namespace - -#endif // ROSSERIAL_SERVER_SESSION_H diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/tcp_server.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/tcp_server.h deleted file mode 100644 index 1dff9c5..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/tcp_server.h +++ /dev/null @@ -1,92 +0,0 @@ -/** - * - * \file - * \brief TCP server for rosserial - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_TCP_SERVER_H -#define ROSSERIAL_SERVER_TCP_SERVER_H - -#include -#include -#include - -#include - -#include "rosserial_server/session.h" - - -namespace rosserial_server -{ - -using boost::asio::ip::tcp; - -template< typename Session = rosserial_server::Session > -class TcpServer -{ -public: - TcpServer(boost::asio::io_service& io_service, short port) - : io_service_(io_service), - acceptor_(io_service, tcp::endpoint(tcp::v4(), port)) - { - start_accept(); - } - -private: - void start_accept() - { - Session* new_session = new Session(io_service_); - acceptor_.async_accept(new_session->socket(), - boost::bind(&TcpServer::handle_accept, this, new_session, - boost::asio::placeholders::error)); - } - - void handle_accept(Session* new_session, - const boost::system::error_code& error) - { - if (!error) - { - new_session->start(); - } - else - { - delete new_session; - } - - start_accept(); - } - - boost::asio::io_service& io_service_; - tcp::acceptor acceptor_; -}; - -} // namespace - -#endif // ROSSERIAL_SERVER_TCP_SERVER_H diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/topic_handlers.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/topic_handlers.h deleted file mode 100644 index 737adb5..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/topic_handlers.h +++ /dev/null @@ -1,195 +0,0 @@ -/** - * - * \file - * \brief Classes which manage the Publish and Subscribe relationships - * that a Session has with its client. - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_TOPIC_HANDLERS_H -#define ROSSERIAL_SERVER_TOPIC_HANDLERS_H - -#include -#include -#include -#include - -namespace rosserial_server -{ - -class Publisher { -public: - Publisher(ros::NodeHandle& nh, const rosserial_msgs::TopicInfo& topic_info) { - rosserial_server::MsgInfo msginfo; - try - { - msginfo = lookupMessage(topic_info.message_type); - } - catch (const std::exception& e) - { - ROS_WARN_STREAM("Unable to look up message: " << e.what()); - } - - if (!msginfo.md5sum.empty() && msginfo.md5sum != topic_info.md5sum) - { - ROS_WARN_STREAM("Message" << topic_info.message_type << "MD5 sum from client does not " - "match that in system. Will avoid using system's message definition."); - msginfo.full_text = ""; - } - message_.morph(topic_info.md5sum, topic_info.message_type, msginfo.full_text, "false"); - publisher_ = message_.advertise(nh, topic_info.topic_name, 1); - } - - void handle(ros::serialization::IStream stream) { - ros::serialization::Serializer::read(stream, message_); - publisher_.publish(message_); - } - - std::string get_topic() { - return publisher_.getTopic(); - } - -private: - ros::Publisher publisher_; - topic_tools::ShapeShifter message_; -}; - -typedef boost::shared_ptr PublisherPtr; - - -class Subscriber { -public: - Subscriber(ros::NodeHandle& nh, rosserial_msgs::TopicInfo& topic_info, - boost::function& buffer)> write_fn) - : write_fn_(write_fn) { - ros::SubscribeOptions opts; - opts.init( - topic_info.topic_name, 1, boost::bind(&Subscriber::handle, this, _1)); - opts.md5sum = topic_info.md5sum; - opts.datatype = topic_info.message_type; - subscriber_ = nh.subscribe(opts); - } - - std::string get_topic() { - return subscriber_.getTopic(); - } - -private: - void handle(const boost::shared_ptr& msg) { - size_t length = ros::serialization::serializationLength(*msg); - std::vector buffer(length); - - ros::serialization::OStream ostream(&buffer[0], length); - ros::serialization::Serializer::write(ostream, *msg); - - write_fn_(buffer); - } - - ros::Subscriber subscriber_; - boost::function& buffer)> write_fn_; -}; - -typedef boost::shared_ptr SubscriberPtr; - -class ServiceClient { -public: - ServiceClient(ros::NodeHandle& nh, rosserial_msgs::TopicInfo& topic_info, - boost::function& buffer, const uint16_t topic_id)> write_fn) - : write_fn_(write_fn) { - topic_id_ = -1; - - rosserial_server::MsgInfo srvinfo; - rosserial_server::MsgInfo reqinfo; - rosserial_server::MsgInfo respinfo; - try - { - srvinfo = lookupMessage(topic_info.message_type, "srv"); - reqinfo = lookupMessage(topic_info.message_type + "Request", "srv"); - respinfo = lookupMessage(topic_info.message_type + "Response", "srv"); - } - catch (const std::exception& e) - { - ROS_WARN_STREAM("Unable to look up service definition: " << e.what()); - } - service_md5_ = srvinfo.md5sum; - request_message_md5_ = reqinfo.md5sum; - response_message_md5_ = respinfo.md5sum; - - ros::ServiceClientOptions opts; - opts.service = topic_info.topic_name; - opts.md5sum = srvinfo.md5sum; - opts.persistent = false; // always false for now - service_client_ = nh.serviceClient(opts); - } - void setTopicId(uint16_t topic_id) { - topic_id_ = topic_id; - } - std::string getServiceMD5() { - return service_md5_; - } - std::string getRequestMessageMD5() { - return request_message_md5_; - } - std::string getResponseMessageMD5() { - return response_message_md5_; - } - - void handle(ros::serialization::IStream stream) { - // deserialize request message - ros::serialization::Serializer::read(stream, request_message_); - - // perform service call - // note that at present, at least for rosserial-windows a service call returns nothing, - // so we discard the return value of this call() invocation. - service_client_.call(request_message_, response_message_, service_md5_); - - // write service response over the wire - size_t length = ros::serialization::serializationLength(response_message_); - std::vector buffer(length); - ros::serialization::OStream ostream(&buffer[0], length); - ros::serialization::Serializer::write(ostream, response_message_); - write_fn_(buffer,topic_id_); - } - -private: - topic_tools::ShapeShifter request_message_; - topic_tools::ShapeShifter response_message_; - ros::ServiceClient service_client_; - boost::function& buffer, const uint16_t topic_id)> write_fn_; - std::string service_md5_; - std::string request_message_md5_; - std::string response_message_md5_; - uint16_t topic_id_; -}; - -typedef boost::shared_ptr ServiceClientPtr; - -} // namespace - -#endif // ROSSERIAL_SERVER_TOPIC_HANDLERS_H diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_socket_session.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_socket_session.h deleted file mode 100644 index 6e38dae..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_socket_session.h +++ /dev/null @@ -1,94 +0,0 @@ -/** - * - * \file - * \brief Reconnecting class for a UDP rosserial session. - * \author Mike Purvis - * \copyright Copyright (c) 2016, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_UDP_SOCKET_SESSION_H -#define ROSSERIAL_SERVER_UDP_SOCKET_SESSION_H - -#include -#include -#include - -#include - -#include "rosserial_server/session.h" -#include "rosserial_server/udp_stream.h" - - -namespace rosserial_server -{ - -using boost::asio::ip::udp; - -class UdpSocketSession : public Session -{ -public: - UdpSocketSession(boost::asio::io_service& io_service, - udp::endpoint server_endpoint, - udp::endpoint client_endpoint) - : Session(io_service), timer_(io_service), - server_endpoint_(server_endpoint), client_endpoint_(client_endpoint) - { - ROS_INFO_STREAM("rosserial_server UDP session created between " << server_endpoint << " and " << client_endpoint); - check_connection(); - } - -private: - void check_connection() - { - if (!is_active()) - { - socket().open(server_endpoint_, client_endpoint_); - start(); - } - - // Every second, check again if the connection should be reinitialized, - // if the ROS node is still up. - if (ros::ok()) - { - timer_.expires_from_now(boost::posix_time::milliseconds(2000)); - timer_.async_wait(boost::bind(&UdpSocketSession::check_connection, this)); - } - else - { - shutdown(); - } - } - - boost::asio::deadline_timer timer_; - udp::endpoint server_endpoint_; - udp::endpoint client_endpoint_; -}; - -} // namespace - -#endif // ROSSERIAL_SERVER_UDP_SOCKET_SESSION_H diff --git a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_stream.h b/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_stream.h deleted file mode 100644 index 30034cb..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/include/rosserial_server/udp_stream.h +++ /dev/null @@ -1,133 +0,0 @@ -/** - * - * \file - * \brief Adapter which allows a single-ended UDP connection to - * present the stream interface. - * \author Mike Purvis - * \copyright Copyright (c) 2016, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#ifndef ROSSERIAL_SERVER_UDP_STREAM_H -#define ROSSERIAL_SERVER_UDP_STREAM_H - -#include -#include -#include - -#include - -#include "rosserial_server/session.h" - - -namespace rosserial_server -{ - -using boost::asio::ip::udp; -#if BOOST_VERSION < 107000 -using boost::asio::handler_type; -#endif - - -class UdpStream : public udp::socket -{ -public: - explicit UdpStream(boost::asio::io_service& io_service) : udp::socket(io_service) - { - } - - void open(udp::endpoint server_endpoint, udp::endpoint client_endpoint) - { - boost::system::error_code ec; - const protocol_type protocol = server_endpoint.protocol(); - - udp::socket::open(protocol, ec); - boost::asio::detail::throw_error(ec, "open"); - - udp::socket::bind(server_endpoint, ec); - boost::asio::detail::throw_error(ec, "bind"); - - client_endpoint_ = client_endpoint; - } - - template - BOOST_ASIO_INITFN_RESULT_TYPE(WriteHandler, - void (boost::system::error_code, std::size_t)) - async_write_some(const ConstBufferSequence& buffers, - BOOST_ASIO_MOVE_ARG(WriteHandler) handler) - { - // If you get an error on the following line it means that your handler does - // not meet the documented type requirements for a WriteHandler. - BOOST_ASIO_WRITE_HANDLER_CHECK(WriteHandler, handler) type_check; -#if (BOOST_VERSION >= 106600) - // See: http://www.boost.org/doc/libs/1_66_0/doc/html/boost_asio/net_ts.html - boost::asio::async_completion init(handler); - - udp::socket::async_send_to( - buffers, client_endpoint_, 0, init.completion_handler); - - return init.result.get(); -#else - return this->get_service().async_send_to( - this->get_implementation(), buffers, client_endpoint_, 0, - BOOST_ASIO_MOVE_CAST(WriteHandler)(handler)); -#endif - } - - template - BOOST_ASIO_INITFN_RESULT_TYPE(ReadHandler, - void (boost::system::error_code, std::size_t)) - async_read_some(const MutableBufferSequence& buffers, - BOOST_ASIO_MOVE_ARG(ReadHandler) handler) - { - // If you get an error on the following line it means that your handler does - // not meet the documented type requirements for a ReadHandler. - BOOST_ASIO_READ_HANDLER_CHECK(ReadHandler, handler) type_check; -#if (BOOST_VERSION >= 106600) - // See: http://www.boost.org/doc/libs/1_66_0/doc/html/boost_asio/net_ts.html - boost::asio::async_completion init(handler); - - udp::socket::async_receive( - buffers, 0, init.completion_handler); - - return init.result.get(); -#else - return this->get_service().async_receive_from( - this->get_implementation(), buffers, client_endpoint_, 0, - BOOST_ASIO_MOVE_CAST(ReadHandler)(handler)); -#endif - } - -private: - udp::endpoint client_endpoint_; -}; - -} // namespace - -#endif // ROSSERIAL_SERVER_UDP_STREAM_H diff --git a/docker/sensor_dep_ros1/rosserial_server/launch/serial.launch b/docker/sensor_dep_ros1/rosserial_server/launch/serial.launch deleted file mode 100644 index 8928429..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/launch/serial.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/docker/sensor_dep_ros1/rosserial_server/launch/socket.launch b/docker/sensor_dep_ros1/rosserial_server/launch/socket.launch deleted file mode 100644 index cde949c..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/launch/socket.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/docker/sensor_dep_ros1/rosserial_server/launch/udp_socket.launch b/docker/sensor_dep_ros1/rosserial_server/launch/udp_socket.launch deleted file mode 100644 index 9b79aed..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/launch/udp_socket.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/docker/sensor_dep_ros1/rosserial_server/package.xml b/docker/sensor_dep_ros1/rosserial_server/package.xml deleted file mode 100644 index df73c39..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - rosserial_server - 0.9.2 - - A more performance- and stability-oriented server alternative implemented - in C++ to rosserial_python. - - - Mike Purvis - Mike Purvis - - BSD - - catkin - rosserial_msgs - std_msgs - roscpp - topic_tools - python3-dev - libboost-thread-dev - libboost-thread - diff --git a/docker/sensor_dep_ros1/rosserial_server/src/msg_lookup.cpp b/docker/sensor_dep_ros1/rosserial_server/src/msg_lookup.cpp deleted file mode 100644 index b2c0e23..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/src/msg_lookup.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/** - * \author Mike Purvis - * \copyright Copyright (c) 2019, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - */ - -#include "rosserial_server/msg_lookup.h" -#include "Python.h" - -namespace rosserial_server -{ - -const MsgInfo lookupMessage(const std::string& message_type, const std::string submodule) -{ - // Lazy-initialize the embedded Python interpreter. Avoid calling the corresponding - // finalize method due to issues with importing cyaml in the second instance. The - // total memory cost of having this in-process is only about 5-6MB. - Py_Initialize(); - - MsgInfo msginfo; - size_t slash_pos = message_type.find('/'); - if (slash_pos == std::string::npos) - { - throw std::runtime_error("Passed message type string does not include a slash character."); - } - std::string module_name = message_type.substr(0, slash_pos); - std::string class_name = message_type.substr(slash_pos + 1, std::string::npos); - - PyObject* module = PyImport_ImportModule((module_name + "." + submodule).c_str()); - if (!module) - { - throw std::runtime_error("Unable to import message module " + module_name + "."); - } - PyObject* msg_class = PyObject_GetAttrString(module, class_name.c_str()); - if (!msg_class) - { - throw std::runtime_error("Unable to find message class " + class_name + - " in module " + module_name + "."); - } - Py_XDECREF(module); - - PyObject* full_text = PyObject_GetAttrString(msg_class, "_full_text"); - PyObject* md5sum = PyObject_GetAttrString(msg_class, "_md5sum"); - if (!md5sum) - { - throw std::runtime_error("Class for message " + message_type + " did not contain" + - "expected _md5sum attribute."); - } - Py_XDECREF(msg_class); - -#if PY_VERSION_HEX > 0x03000000 - if (full_text) - { - msginfo.full_text.assign(PyUnicode_AsUTF8(full_text)); - } - msginfo.md5sum.assign(PyUnicode_AsUTF8(md5sum)); -#else - if (full_text) - { - msginfo.full_text.assign(PyString_AsString(full_text)); - } - msginfo.md5sum.assign(PyString_AsString(md5sum)); -#endif - - // See https://github.com/ros/ros_comm/issues/344 - // and https://github.com/ros/gencpp/pull/14 - // Valid full_text returned, but it is empty, so insert single line - if (full_text && msginfo.full_text.empty()) - { - msginfo.full_text = "\n"; - } - - Py_XDECREF(full_text); - Py_XDECREF(md5sum); - - return msginfo; -} - -} // namespace rosserial_server diff --git a/docker/sensor_dep_ros1/rosserial_server/src/serial_node.cpp b/docker/sensor_dep_ros1/rosserial_server/src/serial_node.cpp deleted file mode 100644 index 8fcd890..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/src/serial_node.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * - * \file - * \brief Main entry point for the serial node. - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#include -#include -#include - -#include - -#include "rosserial_server/serial_session.h" - - -int main(int argc, char* argv[]) -{ - ros::init(argc, argv, "rosserial_server_serial_node"); - - std::string port; - int baud; - ros::param::param("~port", port, "/dev/ttyACM0"); - ros::param::param("~baud", baud, 57600); - - boost::asio::io_service io_service; - rosserial_server::SerialSession serial_session(io_service, port, baud); - io_service.run(); - return 0; -} diff --git a/docker/sensor_dep_ros1/rosserial_server/src/socket_node.cpp b/docker/sensor_dep_ros1/rosserial_server/src/socket_node.cpp deleted file mode 100644 index e803b6d..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/src/socket_node.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * - * \file - * \brief Main entry point for the socket server node. - * \author Mike Purvis - * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#include -#include -#include - -#include - -#include "rosserial_server/tcp_server.h" - - -int main(int argc, char* argv[]) -{ - ros::init(argc, argv, "rosserial_server_socket_node"); - - int port; - ros::param::param("~port", port, 11411); - - boost::asio::io_service io_service; - rosserial_server::TcpServer<> tcp_server(io_service, port); - - ROS_INFO_STREAM("Listening for rosserial TCP connections on port " << port); - io_service.run(); - return 0; -} diff --git a/docker/sensor_dep_ros1/rosserial_server/src/udp_socket_node.cpp b/docker/sensor_dep_ros1/rosserial_server/src/udp_socket_node.cpp deleted file mode 100644 index 1ff7718..0000000 --- a/docker/sensor_dep_ros1/rosserial_server/src/udp_socket_node.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/** - * - * \file - * \brief Main entry point for the UDP socket server node. - * \author Mike Purvis - * \copyright Copyright (c) 2016, Clearpath Robotics, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Clearpath Robotics, Inc. nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com - * - */ - -#include -#include -#include - -#include - -#include "rosserial_server/udp_socket_session.h" - -using boost::asio::ip::udp; -using boost::asio::ip::address; - - -int main(int argc, char* argv[]) -{ - ros::init(argc, argv, "rosserial_server_udp_socket_node"); - - int server_port; - int client_port; - std::string client_addr; - ros::param::param("~server_port", server_port, 11411); - ros::param::param("~client_port", client_port, 11411); - ros::param::param("~client_addr", client_addr, "127.0.0.1"); - - boost::asio::io_service io_service; - rosserial_server::UdpSocketSession udp_socket_session( - io_service, - udp::endpoint(udp::v4(), server_port), - udp::endpoint(address::from_string(client_addr), client_port)); - io_service.run(); - - return 0; -} From 045ac30e6a5c30fb11533e0b81eecd5fb53e7c06 Mon Sep 17 00:00:00 2001 From: robert Date: Wed, 4 Mar 2026 20:39:20 +0800 Subject: [PATCH 17/22] Fix: TF problems - `map` to `odom` frame as static transform --- .../fusion/include/fusion/fusion_node.hpp | 4 ++-- devel/core_algorithm/fusion/src/fusion_node.cpp | 10 +++++----- .../launch/lidar_localization.launch.xml | 4 ++-- .../scripts/lidar_member_function.py | 8 ++++---- .../localization_bringup/launch/localization.launch.py | 4 ++-- devel/utils/include/utils/gen_functs.hpp | 2 +- devel/utils/src/gen_functs.cpp | 2 +- 7 files changed, 17 insertions(+), 17 deletions(-) diff --git a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp index 4da17d9..1d36c71 100644 --- a/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp +++ b/devel/core_algorithm/fusion/include/fusion/fusion_node.hpp @@ -10,7 +10,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.h" #include @@ -39,7 +39,7 @@ class FusionNode : public rclcpp::Node{ rclcpp::Publisher::SharedPtr final_pose_pub; - std::unique_ptr tf_broadcaster; + std::unique_ptr tf_static_broadcaster; std::shared_ptr tf_listener{nullptr}; std::unique_ptr tf_buffer; diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index 068f3cb..6caf1a9 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -8,7 +8,7 @@ #include "utils/enumerators.hpp" FusionNode::FusionNode() : Node("fusion_node"){ - this->tf_broadcaster = std::make_unique(*this); + this->tf_static_broadcaster = std::make_unique(*this); this->tf_buffer = std::make_unique(this->get_clock()); this->tf_listener = std::make_shared(*tf_buffer); @@ -134,7 +134,7 @@ void FusionNode::initCallback(const geometry_msgs::msg::PoseWithCovariance & ini init_tf_msg.rotation.z = init_msg.pose.orientation.z; init_tf_msg.rotation.w = init_msg.pose.orientation.w; - utils::updateTransform(*this->tf_broadcaster, stamp, this->global_frame, this->robot_parent_frame, init_tf_msg); + utils::updateStaticTransform(*this->tf_static_broadcaster, stamp, this->global_frame, this->robot_parent_frame, init_tf_msg); } void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ @@ -226,7 +226,7 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); bool tf_available = utils::indirectUpdateTransform( - *this->tf_broadcaster, + *this->tf_static_broadcaster, *this->tf_buffer, stamp, this->global_frame, @@ -280,7 +280,7 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); bool tf_available = utils::indirectUpdateTransform( - *this->tf_broadcaster, + *this->tf_static_broadcaster, *this->tf_buffer, stamp, this->global_frame, @@ -334,7 +334,7 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); bool tf_available = utils::indirectUpdateTransform( - *this->tf_broadcaster, + *this->tf_static_broadcaster, *this->tf_buffer, stamp, this->global_frame, diff --git a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml index 0198ff6..c190ccf 100644 --- a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml +++ b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml @@ -1,7 +1,7 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py index 94e7f1b..6dcf329 100644 --- a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py +++ b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py @@ -26,7 +26,7 @@ def __init__(self): self.declare_parameter('likelihood_threshold', 0.87) self.declare_parameter('consistency_threshold', 0.93) self.declare_parameter('robot_frame_id', 'base_footprint') - self.declare_parameter('robot_parent_frame_id', 'map') + self.declare_parameter('global_frame_id', 'map') self.declare_parameter('lidar_multiplier', 0.993) self.declare_parameter('likelihood_threshold_two', 0.95) self.declare_parameter('consistency_threshold_two', 0.99) @@ -39,7 +39,7 @@ def __init__(self): self.likelihood_threshold = self.get_parameter('likelihood_threshold').get_parameter_value().double_value self.consistency_threshold = self.get_parameter('consistency_threshold').get_parameter_value().double_value self.robot_frame_id = self.get_parameter('robot_frame_id').get_parameter_value().string_value - self.robot_parent_frame_id = self.get_parameter('robot_parent_frame_id').get_parameter_value().string_value + self.global_frame_id = self.get_parameter('global_frame_id').get_parameter_value().string_value self.lidar_multiplier = self.get_parameter('lidar_multiplier').get_parameter_value().double_value self.likelihood_threshold_two = self.get_parameter('likelihood_threshold_two').get_parameter_value().double_value self.consistency_threshold_two = self.get_parameter('consistency_threshold_two').get_parameter_value().double_value @@ -132,7 +132,7 @@ def obstacle_callback(self, msg): # main # check if TF is available. If true, use the TF. If false, use the latest topic try: self.predict_transform = self.tf_buffer.lookup_transform( - self.robot_parent_frame_id, + self.global_frame_id, self.robot_frame_id, Time() ) @@ -147,7 +147,7 @@ def obstacle_callback(self, msg): # main ) ]) except (LookupException, ConnectivityException, ExtrapolationException) as e: - self.get_logger().error(f'Could not transform {self.robot_parent_frame_id} to {self.robot_frame_id}: {e}') + self.get_logger().error(f'Could not transform {self.global_frame_id} to {self.robot_frame_id}: {e}') self.get_logger().debug("now try to use the latest topic") if self.newPose == False: self.get_logger().error("no new predict topic, skip.") diff --git a/devel/localization_bringup/launch/localization.launch.py b/devel/localization_bringup/launch/localization.launch.py index a09e409..164e9e9 100644 --- a/devel/localization_bringup/launch/localization.launch.py +++ b/devel/localization_bringup/launch/localization.launch.py @@ -170,9 +170,9 @@ def generate_launch_description(): return LaunchDescription([ *declare_args, - TimerAction(period=2.0, actions=[local_launch]), - TimerAction(period=1.5, actions=[global_launch]), TimerAction(period=0.5, actions=[init_pose]), TimerAction(period=1.0, actions=[fusion_launch]), + TimerAction(period=1.5, actions=[global_launch]), + TimerAction(period=2.0, actions=[local_launch]), TimerAction(period=2.5, actions=[rival_node, rival_obstacle_node]) ]) \ No newline at end of file diff --git a/devel/utils/include/utils/gen_functs.hpp b/devel/utils/include/utils/gen_functs.hpp index 58c53c0..1d353fc 100644 --- a/devel/utils/include/utils/gen_functs.hpp +++ b/devel/utils/include/utils/gen_functs.hpp @@ -18,6 +18,6 @@ geometry_msgs::msg::Quaternion yaw2qua(double); void updateStaticTransform(tf2_ros::StaticTransformBroadcaster &, rclcpp::Time, std::string, std::string, geometry_msgs::msg::Transform); void updateTransform(tf2_ros::TransformBroadcaster &, rclcpp::Time, std::string, std::string, geometry_msgs::msg::Transform); -bool indirectUpdateTransform(tf2_ros::TransformBroadcaster &, tf2_ros::Buffer &, rclcpp::Time, std::string, std::string, std::string, geometry_msgs::msg::Transform); +bool indirectUpdateTransform(tf2_ros::StaticTransformBroadcaster &, tf2_ros::Buffer &, rclcpp::Time, std::string, std::string, std::string, geometry_msgs::msg::Transform); } \ No newline at end of file diff --git a/devel/utils/src/gen_functs.cpp b/devel/utils/src/gen_functs.cpp index 591bd6f..932d40c 100644 --- a/devel/utils/src/gen_functs.cpp +++ b/devel/utils/src/gen_functs.cpp @@ -39,7 +39,7 @@ void updateStaticTransform(tf2_ros::StaticTransformBroadcaster & static_tf_br, // @brief directly update transform between two frames void updateTransform( - tf2_ros::TransformBroadcaster & tf_br, + tf2_ros::StaticTransformBroadcaster & tf_br, rclcpp::Time stamp, std::string from_frame, std::string to_frame, From 9477dc30eee9586cf56ae9e41ccdcd9d0a7016c8 Mon Sep 17 00:00:00 2001 From: robert Date: Mon, 9 Mar 2026 22:11:05 +0800 Subject: [PATCH 18/22] Modify: offline localization, normalize quaternion --- .../core_algorithm/fusion/src/fusion_node.cpp | 99 +++++++++---------- .../launch/lidar_localization.launch.xml | 12 ++- .../launch/local.launch.xml | 18 ++-- .../launch/localization.launch.py | 7 ++ .../localization_bringup/param/bag_test.yaml | 7 +- devel/utils/include/utils/gen_functs.hpp | 4 +- devel/utils/src/gen_functs.cpp | 31 +++--- 7 files changed, 97 insertions(+), 81 deletions(-) diff --git a/devel/core_algorithm/fusion/src/fusion_node.cpp b/devel/core_algorithm/fusion/src/fusion_node.cpp index 6caf1a9..5be1df0 100644 --- a/devel/core_algorithm/fusion/src/fusion_node.cpp +++ b/devel/core_algorithm/fusion/src/fusion_node.cpp @@ -7,6 +7,8 @@ #include "utils/gen_functs.hpp" #include "utils/enumerators.hpp" +#include + FusionNode::FusionNode() : Node("fusion_node"){ this->tf_static_broadcaster = std::make_unique(*this); this->tf_buffer = std::make_unique(this->get_clock()); @@ -156,26 +158,33 @@ void FusionNode::localCallback(const nav_msgs::msg::Odometry & local_msg){ 0, local_msg.pose.covariance[7], 0, 0, 0, local_msg.pose.covariance[35]; - geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform(this->global_frame, this->robot_parent_frame, tf2::TimePointZero); + try{ + geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform(this->global_frame, this->robot_parent_frame, tf2::TimePointZero); - Eigen::Vector3d tf_vec; + Eigen::Vector3d tf_vec; - tf_vec(0) = tf.transform.translation.x; - tf_vec(1) = tf.transform.translation.y; - tf_vec(2) = utils::qua2yaw(tf.transform.rotation); + tf_vec(0) = tf.transform.translation.x; + tf_vec(1) = tf.transform.translation.y; + tf_vec(2) = utils::qua2yaw(tf.transform.rotation); - Eigen::Matrix3d tf_rot; - tf_rot << cos(tf_vec(2)), -sin(tf_vec(2)), 0, - sin(tf_vec(2)), cos(tf_vec(2)), 0, - 0, 0, 1; + Eigen::Matrix3d tf_rot; + tf_rot << cos(tf_vec(2)), -sin(tf_vec(2)), 0, + sin(tf_vec(2)), cos(tf_vec(2)), 0, + 0, 0, 1; - Eigen::Vector3d predicted_pose = tf_vec + tf_rot*local_pose; - ekf.Xprediction(predicted_pose, this->local_cov); - - Eigen::Vector3d robot_pose = ekf.getPose(); - Eigen::Matrix3d robot_cov = ekf.getCov(); + Eigen::Vector3d predicted_pose = tf_vec + tf_rot*local_pose; + ekf.Xprediction(predicted_pose, this->local_cov); + + Eigen::Vector3d robot_pose = ekf.getPose(); + Eigen::Matrix3d robot_cov = ekf.getCov(); - pubFinalPose(stamp, robot_pose, robot_cov); + pubFinalPose(stamp, robot_pose, robot_cov); + }catch(const tf2::TransformException & ex){ + RCLCPP_INFO( + this->get_logger(), "Could not transform %s to %s: %s", + this->global_frame.c_str(), this->robot_parent_frame.c_str(), ex.what()); + return; + } } void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & global_msg){ @@ -225,19 +234,13 @@ void FusionNode::globalCallback(const geometry_msgs::msg::PoseWithCovarianceStam robot_tf_msg.translation.z = 0; robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); - bool tf_available = utils::indirectUpdateTransform( - *this->tf_static_broadcaster, - *this->tf_buffer, - stamp, - this->global_frame, - this->robot_parent_frame, - this->robot_frame, - robot_tf_msg); - - if(!tf_available){ - RCLCPP_WARN( - this->get_logger(), "Could not transform %s to %s", - robot_frame.c_str(), robot_parent_frame.c_str()); + std::pair tf_available = utils::indirectUpdateTransform( + *this->tf_static_broadcaster, *this->tf_buffer, stamp, this->global_frame, this->robot_parent_frame, this->robot_frame, robot_tf_msg); + + if(!tf_available.first){ + RCLCPP_INFO( + this->get_logger(), "%s", + tf_available.second.c_str()); } } @@ -279,19 +282,13 @@ void FusionNode::cbCameraCallback(const geometry_msgs::msg::PoseStamped & cbcam_ robot_tf_msg.translation.z = 0; robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); - bool tf_available = utils::indirectUpdateTransform( - *this->tf_static_broadcaster, - *this->tf_buffer, - stamp, - this->global_frame, - this->robot_parent_frame, - this->robot_frame, - robot_tf_msg); - - if(!tf_available){ - RCLCPP_WARN( - this->get_logger(), "Could not transform %s to %s", - robot_frame.c_str(), robot_parent_frame.c_str()); + std::pair tf_available = utils::indirectUpdateTransform( + *this->tf_static_broadcaster, *this->tf_buffer, stamp, this->global_frame, this->robot_parent_frame, this->robot_frame, robot_tf_msg); + + if(!tf_available.first){ + RCLCPP_INFO( + this->get_logger(), "%s", + tf_available.second.c_str()); } } @@ -333,19 +330,13 @@ void FusionNode::obCameraCallback(const geometry_msgs::msg::PoseStamped & obcam_ robot_tf_msg.translation.z = 0; robot_tf_msg.rotation = utils::yaw2qua(robot_pose(2)); - bool tf_available = utils::indirectUpdateTransform( - *this->tf_static_broadcaster, - *this->tf_buffer, - stamp, - this->global_frame, - this->robot_parent_frame, - this->robot_frame, - robot_tf_msg); - - if(!tf_available){ - RCLCPP_WARN( - this->get_logger(), "Could not transform %s to %s", - robot_frame.c_str(), robot_parent_frame.c_str()); + std::pair tf_available = utils::indirectUpdateTransform( + *this->tf_static_broadcaster, *this->tf_buffer, stamp, this->global_frame, this->robot_parent_frame, this->robot_frame, robot_tf_msg); + + if(!tf_available.first){ + RCLCPP_INFO( + this->get_logger(), "%s", + tf_available.second.c_str()); } } diff --git a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml index c190ccf..22cf714 100644 --- a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml +++ b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml @@ -1,15 +1,20 @@ + - + - + - + + + + + @@ -17,6 +22,7 @@ + \ No newline at end of file diff --git a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml index 56826ad..1e80adf 100644 --- a/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml +++ b/devel/core_algorithm/local/wheel_inertial_odometry/launch/local.launch.xml @@ -1,5 +1,6 @@ + @@ -7,16 +8,21 @@ - - - - - - + + + + + + + + + + + \ No newline at end of file diff --git a/devel/localization_bringup/launch/localization.launch.py b/devel/localization_bringup/launch/localization.launch.py index 164e9e9..6956e36 100644 --- a/devel/localization_bringup/launch/localization.launch.py +++ b/devel/localization_bringup/launch/localization.launch.py @@ -7,6 +7,7 @@ def generate_launch_description(): + offline = LaunchConfiguration('offline') global_frame_id = LaunchConfiguration('global_frame_id') robot_parent_frame_id = LaunchConfiguration('robot_parent_frame_id') robot_frame_id = LaunchConfiguration('robot_frame_id') @@ -14,6 +15,10 @@ def generate_launch_description(): rival_name = LaunchConfiguration('rival_name') declare_args = [ + DeclareLaunchArgument( + 'offline', + default_value='false' + ), DeclareLaunchArgument( 'global_frame_id', default_value='map' @@ -68,6 +73,7 @@ def generate_launch_description(): local_launch = IncludeLaunchDescription( AnyLaunchDescriptionSource(local_launch_path), launch_arguments={ + 'offline': offline, 'robot_parent_frame_id': robot_parent_frame_id, 'robot_frame_id': robot_frame_id, }.items() @@ -76,6 +82,7 @@ def generate_launch_description(): global_launch = IncludeLaunchDescription( AnyLaunchDescriptionSource(global_launch_path), launch_arguments={ + 'offline': offline, 'mode': mode }.items() ) diff --git a/devel/localization_bringup/param/bag_test.yaml b/devel/localization_bringup/param/bag_test.yaml index 08acfe6..3c08775 100644 --- a/devel/localization_bringup/param/bag_test.yaml +++ b/devel/localization_bringup/param/bag_test.yaml @@ -1,7 +1,7 @@ bag: mode: play # record or play - path: src/core_algorithm/localization_bringup/bag/ - name: test1 + path: src/devel/localization_bringup/bag/ + name: 0307_beacon_fail_2 record_topics: - /clock - /tf @@ -18,11 +18,8 @@ bag: play_topics: raw: - /clock - - /tf - - /tf_static - /wheel_pose - /scan - - /raw_obstacles - /imu/data_cov result: - /local_pose diff --git a/devel/utils/include/utils/gen_functs.hpp b/devel/utils/include/utils/gen_functs.hpp index 1d353fc..98fb181 100644 --- a/devel/utils/include/utils/gen_functs.hpp +++ b/devel/utils/include/utils/gen_functs.hpp @@ -7,6 +7,8 @@ #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/static_transform_broadcaster.h" +#include + namespace utils { @@ -18,6 +20,6 @@ geometry_msgs::msg::Quaternion yaw2qua(double); void updateStaticTransform(tf2_ros::StaticTransformBroadcaster &, rclcpp::Time, std::string, std::string, geometry_msgs::msg::Transform); void updateTransform(tf2_ros::TransformBroadcaster &, rclcpp::Time, std::string, std::string, geometry_msgs::msg::Transform); -bool indirectUpdateTransform(tf2_ros::StaticTransformBroadcaster &, tf2_ros::Buffer &, rclcpp::Time, std::string, std::string, std::string, geometry_msgs::msg::Transform); +std::pair indirectUpdateTransform(tf2_ros::StaticTransformBroadcaster &, tf2_ros::Buffer &, rclcpp::Time, std::string, std::string, std::string, geometry_msgs::msg::Transform); } \ No newline at end of file diff --git a/devel/utils/src/gen_functs.cpp b/devel/utils/src/gen_functs.cpp index 932d40c..b09ca5a 100644 --- a/devel/utils/src/gen_functs.cpp +++ b/devel/utils/src/gen_functs.cpp @@ -2,6 +2,8 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include + namespace utils { @@ -19,6 +21,7 @@ double qua2yaw(geometry_msgs::msg::Quaternion q){ geometry_msgs::msg::Quaternion yaw2qua(double yaw){ tf2::Quaternion qua; qua.setRPY(0, 0, yaw); + qua.normalize(); return tf2::toMsg(qua); } @@ -39,7 +42,7 @@ void updateStaticTransform(tf2_ros::StaticTransformBroadcaster & static_tf_br, // @brief directly update transform between two frames void updateTransform( - tf2_ros::StaticTransformBroadcaster & tf_br, + tf2_ros::TransformBroadcaster & tf_br, rclcpp::Time stamp, std::string from_frame, std::string to_frame, @@ -59,14 +62,16 @@ void updateTransform( * TF tree: A->B->C. Update TF of AB using AB = AC * BC^-1. * A: from_frame, B: to_frame, C: by_frame, TF of AC: tf_msg. */ -bool indirectUpdateTransform( - tf2_ros::TransformBroadcaster & tf_br, +std::pair indirectUpdateTransform( + tf2_ros::StaticTransformBroadcaster & tf_br, tf2_ros::Buffer & tf_buf, rclcpp::Time stamp, std::string from_frame, std::string to_frame, std::string by_frame, geometry_msgs::msg::Transform tf_msg){ + std::pair result(true, ""); + geometry_msgs::msg::TransformStamped from2to_tf_msg; from2to_tf_msg.header.stamp = stamp; @@ -82,19 +87,21 @@ bool indirectUpdateTransform( try{ to2by_tf_msg = tf_buf.lookupTransform(to_frame, by_frame, tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - return false; - } - tf2::fromMsg(to2by_tf_msg.transform, to2by_tf); - tf2::Transform from2to_tf; - from2to_tf = from2by_tf*to2by_tf.inverse(); + tf2::fromMsg(to2by_tf_msg.transform, to2by_tf); + + tf2::Transform from2to_tf; + from2to_tf = from2by_tf*to2by_tf.inverse(); - from2to_tf_msg.transform = tf2::toMsg(from2to_tf); + from2to_tf_msg.transform = tf2::toMsg(from2to_tf); - tf_br.sendTransform(from2to_tf_msg); + tf_br.sendTransform(from2to_tf_msg); + } catch (const tf2::TransformException & ex) { + result.first = false; + result.second = "Could not get transform " + to_frame + " to " + by_frame + ": " + ex.what(); + } - return true; + return result; } } \ No newline at end of file From e50cfb43301e573a40d5c3a22a67096f64bb456a Mon Sep 17 00:00:00 2001 From: robert Date: Mon, 9 Mar 2026 23:50:40 +0800 Subject: [PATCH 19/22] Add: bot param, lookup timeout --- devel/core_algorithm/fusion/param/fusion.yaml | 4 ++-- devel/core_algorithm/fusion/src/ekf.cpp | 3 +++ .../launch/lidar_localization.launch.xml | 13 ++++++++--- .../param/lidar_localization.yaml | 9 +++++--- .../scripts/lidar_member_function.py | 23 ++++++++++++++----- .../launch/bag_test.launch.py | 2 +- .../launch/localization.launch.py | 6 +++++ .../localization_bringup/param/bag_test.yaml | 4 ++-- devel/utils/src/gen_functs.cpp | 2 +- 9 files changed, 48 insertions(+), 18 deletions(-) diff --git a/devel/core_algorithm/fusion/param/fusion.yaml b/devel/core_algorithm/fusion/param/fusion.yaml index 5784815..58d5aa1 100644 --- a/devel/core_algorithm/fusion/param/fusion.yaml +++ b/devel/core_algorithm/fusion/param/fusion.yaml @@ -1,7 +1,7 @@ fusion_node: ros__parameters: - align_timestamp: false - spin_control: true + align_timestamp: true + spin_control: false spin_threshold: 1.2 diff --git a/devel/core_algorithm/fusion/src/ekf.cpp b/devel/core_algorithm/fusion/src/ekf.cpp index a56e410..740fd85 100644 --- a/devel/core_algorithm/fusion/src/ekf.cpp +++ b/devel/core_algorithm/fusion/src/ekf.cpp @@ -48,6 +48,9 @@ void EKF::correction(Eigen::Vector3d z, Eigen::Matrix3d H, Eigen::Matrix3d Q){ K = this->robot_state.sigma*H.transpose()*(H*this->robot_state.sigma*H.transpose()+Q).inverse(); this->robot_state.mu += K*(z-this->robot_state.mu); + + if(this->robot_state.mu(2) > M_PI || this->robot_state.mu(2) < -M_PI) + this->robot_state.mu(2) = angleNormalizing(this->robot_state.mu(2)); Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); this->robot_state.sigma = (I-K*H)*this->robot_state.sigma; diff --git a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml index 22cf714..167bf61 100644 --- a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml +++ b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml @@ -4,23 +4,30 @@ + + + - + - + - + + diff --git a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yaml b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yaml index 53f323c..f67f0ef 100644 --- a/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yaml +++ b/devel/core_algorithm/global/lidar_localization/param/lidar_localization.yaml @@ -3,6 +3,9 @@ lidar_localization: side: 0 debug_mode: false visualize_candidate: true - likelihood_threshold: 0.85 - consistency_threshold: 0.92 - lidar_multiplier: 0.993 \ No newline at end of file + w_likelihood_threshold: 0.85 + w_consistency_threshold: 0.92 + w_lidar_multiplier: 0.96 + b_likelihood_threshold: 0.87 + b_consistency_threshold: 0.93 + b_lidar_multiplier: 0.993 \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py index 6dcf329..c851475 100644 --- a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py +++ b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py @@ -19,31 +19,42 @@ def __init__(self): super().__init__('lidar_localization_node') # Declare parameters + self.declare_parameter('bot', 'black') self.declare_parameter('mode', 'yellow') self.declare_parameter('debug_mode', False) self.declare_parameter('use_two_beacons', False) self.declare_parameter('visualize_candidate', True) - self.declare_parameter('likelihood_threshold', 0.87) - self.declare_parameter('consistency_threshold', 0.93) self.declare_parameter('robot_frame_id', 'base_footprint') self.declare_parameter('global_frame_id', 'map') - self.declare_parameter('lidar_multiplier', 0.993) + self.declare_parameter('w_likelihood_threshold', 0.87) + self.declare_parameter('w_consistency_threshold', 0.93) + self.declare_parameter('w_lidar_multiplier', 0.87) + self.declare_parameter('b_likelihood_threshold', 0.87) + self.declare_parameter('b_consistency_threshold', 0.93) + self.declare_parameter('b_lidar_multiplier', 0.993) self.declare_parameter('likelihood_threshold_two', 0.95) self.declare_parameter('consistency_threshold_two', 0.99) # Get parameters + self.bot = self.get_parameter('bot').get_parameter_value().string_value self.side = self.get_parameter('mode').get_parameter_value().string_value self.debug_mode = self.get_parameter('debug_mode').get_parameter_value().bool_value self.p_use_two_beacons = self.get_parameter('use_two_beacons').get_parameter_value().bool_value self.visualize_true = self.get_parameter('visualize_candidate').get_parameter_value().bool_value - self.likelihood_threshold = self.get_parameter('likelihood_threshold').get_parameter_value().double_value - self.consistency_threshold = self.get_parameter('consistency_threshold').get_parameter_value().double_value self.robot_frame_id = self.get_parameter('robot_frame_id').get_parameter_value().string_value self.global_frame_id = self.get_parameter('global_frame_id').get_parameter_value().string_value - self.lidar_multiplier = self.get_parameter('lidar_multiplier').get_parameter_value().double_value self.likelihood_threshold_two = self.get_parameter('likelihood_threshold_two').get_parameter_value().double_value self.consistency_threshold_two = self.get_parameter('consistency_threshold_two').get_parameter_value().double_value + if self.bot == 'white': + self.likelihood_threshold = self.get_parameter('w_likelihood_threshold').get_parameter_value().double_value + self.consistency_threshold = self.get_parameter('w_consistency_threshold').get_parameter_value().double_value + self.lidar_multiplier = self.get_parameter('w_lidar_multiplier').get_parameter_value().double_value + elif self.bot == 'black': + self.likelihood_threshold = self.get_parameter('b_likelihood_threshold').get_parameter_value().double_value + self.consistency_threshold = self.get_parameter('b_consistency_threshold').get_parameter_value().double_value + self.lidar_multiplier = self.get_parameter('w_lidar_multiplier').get_parameter_value().double_value + # Set the landmarks map based on the side if self.side == 'yellow': self.landmarks_map = [ diff --git a/devel/localization_bringup/launch/bag_test.launch.py b/devel/localization_bringup/launch/bag_test.launch.py index 0aeb489..6ca83de 100644 --- a/devel/localization_bringup/launch/bag_test.launch.py +++ b/devel/localization_bringup/launch/bag_test.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): remaps = [] for topic in play_pose_topics: - remaps.append(f'{topic}:=/eurobot2025{topic}') + remaps.append(f'{topic}:=/pose_info{topic}') actions = [] diff --git a/devel/localization_bringup/launch/localization.launch.py b/devel/localization_bringup/launch/localization.launch.py index 6956e36..3126c5a 100644 --- a/devel/localization_bringup/launch/localization.launch.py +++ b/devel/localization_bringup/launch/localization.launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): global_frame_id = LaunchConfiguration('global_frame_id') robot_parent_frame_id = LaunchConfiguration('robot_parent_frame_id') robot_frame_id = LaunchConfiguration('robot_frame_id') + bot = LaunchConfiguration('bot') mode = LaunchConfiguration('mode') rival_name = LaunchConfiguration('rival_name') @@ -31,6 +32,10 @@ def generate_launch_description(): 'robot_frame_id', default_value='base_footprint' ), + DeclareLaunchArgument( + 'bot', + default_value='black' + ), DeclareLaunchArgument( 'mode', default_value='yellow' @@ -82,6 +87,7 @@ def generate_launch_description(): global_launch = IncludeLaunchDescription( AnyLaunchDescriptionSource(global_launch_path), launch_arguments={ + 'bot': bot, 'offline': offline, 'mode': mode }.items() diff --git a/devel/localization_bringup/param/bag_test.yaml b/devel/localization_bringup/param/bag_test.yaml index 3c08775..a7de3a6 100644 --- a/devel/localization_bringup/param/bag_test.yaml +++ b/devel/localization_bringup/param/bag_test.yaml @@ -1,7 +1,7 @@ bag: mode: play # record or play path: src/devel/localization_bringup/bag/ - name: 0307_beacon_fail_2 + name: 0308_beacon_fail record_topics: - /clock - /tf @@ -10,7 +10,7 @@ bag: - /scan - /raw_obstacles - /imu/data_cov - - /local_filter + - /local_pose - /lidar_pose - /final_pose - /beacons_guaguagua diff --git a/devel/utils/src/gen_functs.cpp b/devel/utils/src/gen_functs.cpp index b09ca5a..5db700d 100644 --- a/devel/utils/src/gen_functs.cpp +++ b/devel/utils/src/gen_functs.cpp @@ -86,7 +86,7 @@ std::pair indirectUpdateTransform( geometry_msgs::msg::TransformStamped to2by_tf_msg; try{ - to2by_tf_msg = tf_buf.lookupTransform(to_frame, by_frame, tf2::TimePointZero); + to2by_tf_msg = tf_buf.lookupTransform(to_frame, by_frame, stamp, tf2::durationFromSec(0.05)); tf2::fromMsg(to2by_tf_msg.transform, to2by_tf); From 06e3b1cf476c5a9dc83dc75c21f553e080aec3a3 Mon Sep 17 00:00:00 2001 From: robert Date: Tue, 10 Mar 2026 01:03:55 +0800 Subject: [PATCH 20/22] Modify: normalize quaternion --- devel/core_algorithm/fusion/src/ekf.cpp | 16 ++-- .../lidar_localization/beacon_finder.hpp | 7 ++ .../lidar_localization_node.hpp | 43 +++++++++++ .../lidar_localization/trilateration.hpp | 0 .../launch/lidar_localization.launch.xml | 2 +- .../scripts/lidar_member_function.py | 14 ++-- .../lidar_localization/src/beacon_finder.cpp | 0 .../src/lidar_localization_node.cpp | 77 +++++++++++++++++++ .../lidar_localization/src/trilateration.cpp | 0 devel/utils/src/gen_functs.cpp | 1 + 10 files changed, 147 insertions(+), 13 deletions(-) create mode 100644 devel/core_algorithm/global/lidar_localization/include/lidar_localization/beacon_finder.hpp create mode 100644 devel/core_algorithm/global/lidar_localization/include/lidar_localization/lidar_localization_node.hpp create mode 100644 devel/core_algorithm/global/lidar_localization/include/lidar_localization/trilateration.hpp create mode 100644 devel/core_algorithm/global/lidar_localization/src/beacon_finder.cpp create mode 100644 devel/core_algorithm/global/lidar_localization/src/lidar_localization_node.cpp create mode 100644 devel/core_algorithm/global/lidar_localization/src/trilateration.cpp diff --git a/devel/core_algorithm/fusion/src/ekf.cpp b/devel/core_algorithm/fusion/src/ekf.cpp index 740fd85..fd15df2 100644 --- a/devel/core_algorithm/fusion/src/ekf.cpp +++ b/devel/core_algorithm/fusion/src/ekf.cpp @@ -31,10 +31,16 @@ void EKF::Vprediction(Eigen::Vector3d u, Eigen::Matrix3d R, double dt){ double sin_dt = sin(u(2)*dt); Eigen::Matrix3d d_state; - d_state << (sin_theta*(cos_dt-1)+cos_theta*sin_dt)/u(2), (cos_theta*(cos_dt-1)-sin_theta*sin_dt)/u(2), 0, - -(cos_theta*(cos_dt-1)-sin_theta*sin_dt)/u(2), (sin_theta*(cos_dt-1)+cos_theta*sin_dt)/u(2), 0, - 0, 0, dt; - + if(abs(u(2))>0.001){ + d_state << (sin_theta*(cos_dt-1)+cos_theta*sin_dt)/u(2), (cos_theta*(cos_dt-1)-sin_theta*sin_dt)/u(2), 0, + -(cos_theta*(cos_dt-1)-sin_theta*sin_dt)/u(2), (sin_theta*(cos_dt-1)+cos_theta*sin_dt)/u(2), 0, + 0, 0, dt; + } + else { + d_state << cos(this->robot_state.mu(2))*dt, -sin(this->robot_state.mu(2))*dt, 0, + sin(this->robot_state.mu(2))*dt, cos(this->robot_state.mu(2))*dt, 0, + 0, 0, 1; + } this->robot_state.mu += d_state*u; if(this->robot_state.mu(2) > M_PI || this->robot_state.mu(2) < -M_PI) @@ -48,7 +54,7 @@ void EKF::correction(Eigen::Vector3d z, Eigen::Matrix3d H, Eigen::Matrix3d Q){ K = this->robot_state.sigma*H.transpose()*(H*this->robot_state.sigma*H.transpose()+Q).inverse(); this->robot_state.mu += K*(z-this->robot_state.mu); - + if(this->robot_state.mu(2) > M_PI || this->robot_state.mu(2) < -M_PI) this->robot_state.mu(2) = angleNormalizing(this->robot_state.mu(2)); diff --git a/devel/core_algorithm/global/lidar_localization/include/lidar_localization/beacon_finder.hpp b/devel/core_algorithm/global/lidar_localization/include/lidar_localization/beacon_finder.hpp new file mode 100644 index 0000000..4572533 --- /dev/null +++ b/devel/core_algorithm/global/lidar_localization/include/lidar_localization/beacon_finder.hpp @@ -0,0 +1,7 @@ +#ifndef BEACON_FINDER_HPP_ +#define BEACON_FINDER_HPP_ + +struct CoordStruct{ + double x; + double y; +}; \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/include/lidar_localization/lidar_localization_node.hpp b/devel/core_algorithm/global/lidar_localization/include/lidar_localization/lidar_localization_node.hpp new file mode 100644 index 0000000..e08e796 --- /dev/null +++ b/devel/core_algorithm/global/lidar_localization/include/lidar_localization/lidar_localization_node.hpp @@ -0,0 +1,43 @@ +#ifndef LIDAR_LOCALIZATION_NODE_HPP_ +#define LIDAR_LOCALIZATION_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "obstacle_detector/msg/obstacle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" + +#include + +class LidarLocalizationNode : public rclcpp::Node{ + public: + LidarNode(); + + void obstacleCallback(const obstacle_detector::msg::Obstacle &); + void predCallback(const nav_msgs::msg::Odometry &); + + void pubGlobalPoser(rclcpp::Time, Eigen::Vector3d, Eigen::Matrix3d); + void pubBeacons(); + + private: + rclcpp::Subscription obs_sub; + rclcpp::Subscription pred_sub; + + rclcpp::Puslisher global_puse_pub; + rclcpp::Publisher beacons_pub; + + std::string mode; + std::string global_frame; + std::string robot_frame; + + std::list + + double likelihood_threshold; + double consistency_threshold; + double lidar_fac; + + double beacon_coord[3][2]; +} + +#endif //LIDAR_LOCALIZATION_NODE_HPP_ \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/include/lidar_localization/trilateration.hpp b/devel/core_algorithm/global/lidar_localization/include/lidar_localization/trilateration.hpp new file mode 100644 index 0000000..e69de29 diff --git a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml index 167bf61..70154ff 100644 --- a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml +++ b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml @@ -8,7 +8,7 @@ - + tf_buffer = std::make_unique(this->get_clock()); + this->tf_listener = std::make_shared(*tf_buffer); + + + this->declare_parameter("mode", "yellow"); + this->mode = this->get_parameter("mode").as_string(); + + this->declare_parameter("global_frame_id", "map"); + this->global_frame = this->get_parameter("global_frame_id").as_string(); + + this->declare_parameter("robot_frame_id", "base_footprint"); + this->robot_frame = this->get_parameter("robot_frame_id").as_string(); + + this->declare_parameter("likelihood_threshold", 0.87); + double likelihood_threshold = this->get_parameter("likelihood_threshold").as_double(); + + this->declare_parameter("consistency_threshold", 0.93); + double consistency_threshold = this->get_parameter("consistency_threshold").as_double(); + + this->declare_parameter("lidar_multiplier", 0.987); + double lidar_fac = this->get_parameter("lidar_multiplier").as_double(); + + if(this->mode == "yellow"){ + this->beacon_coord[0][0] = -0.083; this->beacon_coord[0][1] = 0.041; + this->beacon_coord[1][0] = -0.083; this->beacon_coord[1][1] = 1.959; + this->beacon_coord[2][0] = 3.083; this->beacon_coord[2][1] = 1.0; + } + else if(this->mode == "blue"){ + this->beacon_coord[0][0] = 3.083; this->beacon_coord[0][1] = 0.041; + this->beacon_coord[1][0] = 3.083; this->beacon_coord[1][1] = 1.959; + this->beacon_coord[2][0] = -0.083; this->beacon_coord[2][1] = 1.0; + } + + + this->obs_sub = this->create_subscription( + "raw_obstacles", 10, std::bind(&LidarLocalizationNode::obstacleCallback, this, std::placeholders::_1)); + + this->pred_sub = this->create_subscription( + "final_pose", 10, std::bind(&LidarLocalizationNode::predCallback, this, std::placeholders::_1)); + + + this->global_pose_pub = this->create_publisher("global_pose", 10); + + this->beacons_pub = this->create_publisher("beacons", 10); +} + +LidarLocalizationNode::obstacleCallback(const obstacle_detector::msg::Obstacles & obs_msg){ + // assign obstacle coordinates to linked list + int circles_len = sizeof(obs_msg.circles)/sizeof(obs_msg.circles[0]); + for(int i=0; i()); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/devel/core_algorithm/global/lidar_localization/src/trilateration.cpp b/devel/core_algorithm/global/lidar_localization/src/trilateration.cpp new file mode 100644 index 0000000..e69de29 diff --git a/devel/utils/src/gen_functs.cpp b/devel/utils/src/gen_functs.cpp index 5db700d..d8a0c90 100644 --- a/devel/utils/src/gen_functs.cpp +++ b/devel/utils/src/gen_functs.cpp @@ -15,6 +15,7 @@ double qua2yaw(geometry_msgs::msg::Quaternion q){ tf2::Quaternion qua(q.x, q.y, q.z, q.w); double _, yaw; tf2::Matrix3x3(qua).getRPY(_, _, yaw); + angleNormalizing(yaw); return yaw; } From c412f03d01eb927b4ec14a10cc0d1f4913626dbe Mon Sep 17 00:00:00 2001 From: robert Date: Tue, 10 Mar 2026 01:30:25 +0800 Subject: [PATCH 21/22] Add: bot argument when compose up --- .../lidar_localization/launch/lidar_localization.launch.xml | 6 +++--- .../lidar_localization/scripts/lidar_member_function.py | 4 ++-- devel/localization_bringup/launch/localization.launch.py | 2 +- docker/docker-compose.yaml | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml index 70154ff..df93b7f 100644 --- a/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml +++ b/devel/core_algorithm/global/lidar_localization/launch/lidar_localization.launch.xml @@ -4,12 +4,12 @@ - + - - + + Date: Tue, 10 Mar 2026 01:42:53 +0800 Subject: [PATCH 22/22] Fix: bot argument type --- .../lidar_localization/scripts/lidar_member_function.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py index e5decc8..03c858d 100644 --- a/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py +++ b/devel/core_algorithm/global/lidar_localization/scripts/lidar_member_function.py @@ -19,7 +19,7 @@ def __init__(self): super().__init__('lidar_localization_node') # Declare parameters - self.declare_parameter('bot', 'black') + self.declare_parameter('bot', 13) self.declare_parameter('mode', 'yellow') self.declare_parameter('debug_mode', False) self.declare_parameter('use_two_beacons', False) @@ -36,7 +36,7 @@ def __init__(self): self.declare_parameter('consistency_threshold_two', 0.99) # Get parameters - self.bot = self.get_parameter('bot').get_parameter_value().string_value + self.bot = self.get_parameter('bot').get_parameter_value().integer_value self.side = self.get_parameter('mode').get_parameter_value().string_value self.debug_mode = self.get_parameter('debug_mode').get_parameter_value().bool_value self.p_use_two_beacons = self.get_parameter('use_two_beacons').get_parameter_value().bool_value @@ -46,11 +46,11 @@ def __init__(self): self.likelihood_threshold_two = self.get_parameter('likelihood_threshold_two').get_parameter_value().double_value self.consistency_threshold_two = self.get_parameter('consistency_threshold_two').get_parameter_value().double_value - if self.bot == '11': + if self.bot == 11: self.likelihood_threshold = self.get_parameter('w_likelihood_threshold').get_parameter_value().double_value self.consistency_threshold = self.get_parameter('w_consistency_threshold').get_parameter_value().double_value self.lidar_multiplier = self.get_parameter('w_lidar_multiplier').get_parameter_value().double_value - elif self.bot == '13': + elif self.bot == 13: self.likelihood_threshold = self.get_parameter('b_likelihood_threshold').get_parameter_value().double_value self.consistency_threshold = self.get_parameter('b_consistency_threshold').get_parameter_value().double_value self.lidar_multiplier = self.get_parameter('b_lidar_multiplier').get_parameter_value().double_value