diff --git a/README.md b/README.md index 070dc8ad1a1200ba674b91ddf6d4b58e9c39f2c4..38651e571f134b21a7c2602e687d5e6e041f1131 100644 --- a/README.md +++ b/README.md @@ -1,67 +1,99 @@ # PB_RM_Simulation 深圳北理莫斯科大学 北极熊战队 哨兵导航仿真包 -## 1. 项目介绍 -本项目使用麦克纳姆轮仿真小车，附加 Livox mid360 雷达与 IMU，在 2023 RMUC 地图进行导航算法仿真。 +## 一. 项目介绍 +本项目使用麦克纳姆轮仿真小车，附加 Livox Mid360 雷达与 Imu，在 2023 RMUC/RMUL 地图进行导航算法仿真。 -仿真小车：基于 [华农 Taurus 哨兵定位导航算法](https://github.com/SCAU-RM-NAV/rm2023_auto_sentry_ws) 的 simple_meca_car 修改，将其升级为 ROS2 仿真包 - -mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livox-SDK/livox_laser_simulation/blob/main/src/livox_points_plugin.cpp)、 [livox_laser_simulation_RO2](https://github.com/stm32f303ret6/livox_laser_simulation_RO2/blob/main/src/livox_points_plugin.cpp)、 [Issue15: CustomMsg](https://github.com/Livox-SDK/livox_laser_simulation/issues/15) +Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livox-SDK/livox_laser_simulation/blob/main/src/livox_points_plugin.cpp)、 [livox_laser_simulation_RO2](https://github.com/stm32f303ret6/livox_laser_simulation_RO2/blob/main/src/livox_points_plugin.cpp)、 [Issue15: CustomMsg](https://github.com/Livox-SDK/livox_laser_simulation/issues/15) 导航算法框架：基于 [中南大学 FYT 战队 RM 哨兵上位机算法 ](https://github.com/baiyeweiguang/CSU-RM-Sentry) 略微修改以适配仿真。 非常非常感谢以上开源项目的帮助！！！ +|Gazebo 仿真|Fast_LIO + Navigation2| +|:-:|:-:| +||| -![Gazebo_Simulation](docs/Gazebo_Simulation.png) -![FAST_LIO+Nav2](docs/FAST_LIO+Nav2.png) +|Mid360点云仿真 + FAST_LIO 里程计 + 障碍物/地面点云分割 + 三维点云转二维 + Navigation2| +|:-:| +|| -## 2. rm_simulation 话题接口 +## 二. rm_simulation 话题接口 | **Topic name** | **Type** | **Note** | |:-------------------:|:-------------------------------:|:--------------------------------:| -| /mid360 | livox_ros_driver2/msg/CustomMsg | Mid360 自定义消息类型，用于 FAST_LIO 等算法 | -| /mid360_PointCloud2 | sensor_msgs/msg/PointCloud2 | ROS2 通用点云格式 | -| /imu | sensor_msgs/msg/Imu | Gazebo 插件仿真 IMU | -| /cmd_vel | geometry_msgs/msg/Twist | 麦克纳姆轮小车运动控制话题接口 | -| /odom | nav_msgs/msg/Odometry | Gazebo mecanum_controller 输出的里程计 | - - -## 3. 环境配置 -当前开发分支为 ROS2 humble, Gazebo 11.10.0 - -``` -sudo apt-get install ros-humble-gazebo-* -sudo apt-get install ros-humble-xacro -sudo apt-get install ros-humble-robot-state-publisher -sudo apt-get install ros-humble-joint-state-publisher -sudo apt-get install ros-humble-rviz2 -sudo apt-get install libboost-all-dev -``` - -``` -sudo apt install -y ros-humble-nav2* -sudo apt install -y ros-humble-pcl-ros -sudo apt install -y ros-humble-pcl-conversions -sudo apt install -y ros-humble-libpointmatcher -sudo apt install -y ros-humble-tf2-geometry-msgs -sudo apt install -y libgoogle-glog-dev -``` -后续编译与运行过程中如果有依赖功能包未安装，按类似的方法安装。 +| /livox/lidar | livox_ros_driver2/msg/CustomMsg | Mid360 自定义消息类型 | +| /livox/lidar_PointCloud2 | sensor_msgs/msg/PointCloud2 | ROS2 点云消息类型 | +| /livox/imu | sensor_msgs/msg/Imu | Gazebo 插件仿真 IMU | +| /cmd_vel | geometry_msgs/msg/Twist | 麦克纳姆轮小车运动控制接口 | +| /odom | nav_msgs/msg/Odometry | FAST_LIO 输出的里程计 | + + +## 三. 环境配置 +当前开发环境为 Ubuntu22.04, ROS2 humble, Gazebo 11.10.0 + +1. 安装依赖 + + ```bash + sudo apt install -y ros-humble-gazebo-* + sudo apt install -y ros-humble-xacro + sudo apt install -y ros-humble-robot-state-publisher + sudo apt install -y ros-humble-joint-state-publisher + sudo apt install -y ros-humble-rviz2 + sudo apt install -y ros-humble-nav2* + sudo apt install -y ros-humble-pcl-ros + sudo apt install -y ros-humble-pcl-conversions + sudo apt install -y ros-humble-libpointmatcher + sudo apt install -y ros-humble-tf2-geometry-msgs + sudo apt install -y libboost-all-dev + sudo apt install -y libgoogle-glog-dev + ``` + + 后续编译与运行过程中如果有依赖功能包未安装，按类似的方法安装。 + +2. 安装 [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) + + ```bash + sudo apt install cmake + ``` + + ```bash + git clone https://github.com/Livox-SDK/Livox-SDK2.git + cd ./Livox-SDK2/ + mkdir build + cd build + cmake .. && make -j + sudo make install + ``` +3. 安装 [Livox ROS Driver 2](https://github.com/Livox-SDK/Livox-SDK) + + ``` + git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 + ``` + + ``` + cd ws_livox/src/livox_ros_driver2 + source /opt/ros/humble/setup.sh + ./build.sh humble + ``` + + 设置每次启动终端时，自动 source livox_ros_driver2 的工作空间，才能使用 livox 雷达的自定义消息类型。**注意替换 `WHERE_YOU_GIT_CLONE`** + ``` + echo '# livox_ros_driver2' >> ~/.bashrc + echo 'source source /WHERE_YOU_GIT_CLONE/ws_livox/install/setup.bash' >> ~/.bashrc + ``` + +## 四. 编译运行 ``` git clone https://gitee.com/SMBU-POLARBEAR/pb_rmsimulation ``` -## 4. 编译与运行 - -请注意，编译本项目前，需要先 source livox_ros_driver2 软件包的工作空间，详见 [FAST_LIO](https://github.com/Ericsii/FAST_LIO#13-livox_ros_driver2) 的编译教程 - #### 边建图边导航 ``` ./mapping.sh ``` -#### 边建图边导航 +#### 已知全局地图导航 ``` ./nav.sh ``` @@ -73,11 +105,11 @@ source install/setup.bash ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` -## 5. 地图选择 +## 五. 地图选择 | 地图 | 描述 | 来源 | |:---------------------:|:----------:|:---:| -| auto_world.world | 2022 RMUL-步兵对抗 地图 | [华农](https://github.com/SCAU-RM-NAV/rm2022_auto_infantry_ws) | +| auto_world | 2022 RMUL-步兵对抗 地图 | [华农](https://github.com/SCAU-RM-NAV/rm2022_auto_infantry_ws) | | RMUC2023_world | 2023 RMUC 地图 | [Robomaster 官方](https://bbs.robomaster.com/thread-22576-1-1.html) | 当前默认地图为 `RMUC2023_world`
@@ -88,21 +120,35 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard `ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUC`
or
`ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUL` -## 6. 雷达选择 -目前支持 livox 系列部分雷达: -avia, HAP, horizon, mid40, mid70, mid360, tele 雷达。 +## 六. 雷达选择 +目前支持 Livox 系列雷达: +Avia, HAP, Horizon, Mid40, Mid70, Mid360, Tele -当前默认使用 mid360 仿真，如需更换其它 Livox 系列雷达进行仿真，请在 [waking_robot.xacro](/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro) 中仿照下述样式修改 +当前默认使用 Mid360 仿真，如需更换其它 Livox 系列雷达进行仿真，请在 [waking_robot.xacro](/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro) 中仿照下述样式修改 ```xml - - + + ``` -## 7. TODO +## ATTENTION 当前项目存在性能问题 +1. 仿真性能瓶颈，livox_laser_simulation 无法多核并行计算，导致一核有难，十核围观。在 RMUC 地图中 mapping 模式 的 /livox/lidar 的频率仅 3.6~4.1Hz，在 nav 模式中频率更低。 + +2. 2023.12.8 版本往后修复了之前错误的 tf 树，现在的里程计完全由 FAST_LIO 提供，在 RMUC 地图中 mapping 模式 的 /Odometry 的频率仅约 3.8Hz ，远远不及使用 Gazebo libgazebo_ros_planar_move 直接提供的 里程计（50~60Hz）），这导致 navigation 控制器发出的小车运动速度很小。 + +3. 在 nav 仿真模式下，运行 icp_localization 时存在概率性中断，抛出以下报错： + ```bash + [icp_localization-1] terminate called after throwing an instance of 'std::runtime_error' + [icp_localization-1] what(): val: 621356012573094357 is not ge than: 621356012573096939 + [ERROR] [icp_localization-1]: process has died [pid 48771, exit code -6, cmd '/home/CLH_NAVIGATION_WS/pb_rmsimulation/install/icp_localization_ros2/lib/icp_localization_ros2/icp_localization --ros-args --params-file /home/CLH_NAVIGATION_WS/pb_rmsimulation/install/icp_localization_ros2/share/icp_localization_ros2/config/node_params.yaml --params-file /tmp/launch_params_rtjf6lwa']. + ``` + 在多台主机上的测试表明，这可能与主机性能有关，尤其是内存大小。 + 内存为 32GB 的 NUC 中断概率较小，而在 16GB 的拯救者在 WSL2 下仅运行 5 秒即中断。 + +## TODO 1. ~~将湖工大地图模型文件修改为动态路径~~（2023.9.30 完成） 2. ~~多线雷达~~（2023.10.8 完成） @@ -114,3 +160,5 @@ avia, HAP, horizon, mid40, mid70, mid360, tele 雷达。 5. ~~优雅的选择地图，并根据不同地图适配小车出生点~~（2023.11.25完成） 6. 决策树 + +7. 优化 livox_laser_simulation 性能问题 diff --git a/docs/RMUL_mapping.gif b/docs/RMUL_mapping.gif new file mode 100644 index 0000000000000000000000000000000000000000..b8d9251080fd145bba058464dc9143259fe506c5 Binary files /dev/null and b/docs/RMUL_mapping.gif differ diff --git a/mapping.sh b/mapping.sh index 3f2293b112be26f7d5e9934123f0aba6455764e1..6d1be23d088dc2161ffd21800568edfa44dfda51 100755 --- a/mapping.sh +++ b/mapping.sh @@ -1,10 +1,28 @@ colcon build --symlink-install -cmds=( "ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUC" - "ros2 launch linefit_ground_segmentation_ros segmentation.launch.py" - "ros2 launch fast_lio mapping_mid360.launch.py rviz:=false" - "ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py" - "ros2 launch rm_navigation online_async_launch.py" - "ros2 launch rm_navigation bringup_no_amcl_launch.py") + +cmds=( + "ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUC \ + use_sim_time:=true + " + "ros2 launch imu_complementary_filter complementary_filter.launch.py \ + use_sim_time:=true + " + "ros2 launch fast_lio mapping_mid360.launch.py \ + use_sim_time:=true rviz:=false + " + "ros2 launch linefit_ground_segmentation_ros segmentation.launch.py \ + use_sim_time:=true + " + "ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py \ + use_sim_time:=true + " + "ros2 launch rm_navigation online_async_launch.py \ + use_sim_time:=true + " + "ros2 launch rm_navigation bringup_no_amcl_launch.py \ + use_sim_time:=true + " +) for cmd in "${cmds[@]}" do diff --git a/nav.sh b/nav.sh index db657ffa737f9e0fc36e458bbd1cd5e390780bda..5614d1551b268b1a0279532666b457490b99298d 100755 --- a/nav.sh +++ b/nav.sh @@ -1,12 +1,30 @@ colcon build --symlink-install -cmds=( "ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUC" - "ros2 launch linefit_ground_segmentation_ros segmentation.launch.py" - "ros2 launch fast_lio mapping_mid360.launch.py rviz:=false" - "ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py" - "ros2 launch icp_localization_ros2 bringup.launch.py" - "ros2 launch rm_navigation bringup_launch.py") -for cmd in "${cmds[@]}"; +cmds=( + "ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUC \ + use_sim_time:=true + " + "ros2 launch imu_complementary_filter complementary_filter.launch.py \ + use_sim_time:=true + " + "ros2 launch fast_lio mapping_mid360.launch.py \ + use_sim_time:=true rviz:=false + " + "ros2 launch linefit_ground_segmentation_ros segmentation.launch.py \ + use_sim_time:=true + " + "ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py \ + use_sim_time:=true + " + "ros2 launch icp_localization_ros2 bringup.launch.py \ + use_sim_time:=true + " + "ros2 launch rm_navigation bringup_launch.py \ + use_sim_time:=true + " +) + +for cmd in "${cmds[@]}" do echo Current CMD : "$cmd" gnome-terminal -- bash -c "cd $(pwd);source install/setup.bash;$cmd;exec bash;" diff --git a/src/rm_decision/README.md b/src/rm_decision/README.md index b80f923e5d3c79eeb9061510bbd6170c707a42ab..659a6591aaa0d4a14303ffd0042566c858978b1e 100644 --- a/src/rm_decision/README.md +++ b/src/rm_decision/README.md @@ -77,8 +77,6 @@ ros2 launch rm_decision pb_auto_fsm_launch.py ros2 topic pub -r 5 /robot_status rm_decision_interfaces/msg/RobotStatus "{ robot_id: 7, current_hp: 200, - maximum_hp: 200, - shooter_heat_limit: 50, shooter_heat: 19, }" ``` @@ -138,7 +136,12 @@ ros2 launch rm_decision pb_auto_fsm_launch.py orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} } }" - ``` - +- RFID + ``` + source install/setup.bash + ros2 topic pub -r 5 /rfid rm_decision_interfaces/msg/RFID "{ + rfid_patrol_status: 0 + }" + ``` diff --git a/src/rm_decision/rm_decision/callback_msg.py b/src/rm_decision/rm_decision/callback_msg.py index 3a8a574d64d7f026ac0cdea1511829635bed79f8..6a328e11542675e3386ba6420440b5ff192e96d7 100644 --- a/src/rm_decision/rm_decision/callback_msg.py +++ b/src/rm_decision/rm_decision/callback_msg.py @@ -1,6 +1,12 @@ from rclpy.node import Node from rclpy import logging +from enum import Enum + +class TeamColor(Enum): + RED = 0 + BLUE = 1 + class CallbackMsg(Node): """ 自定义消息回调类 @@ -10,28 +16,32 @@ class CallbackMsg(Node): self.friend_color = 0 # 0-Red, 1-Blue # Robot HP （1-英雄 2-工程 3-步兵 4-步兵 5-步兵 7-哨兵 8-前哨站 9-基地） - self.friend_robot_HP = [0] * 10; - self.enemy_robot_HP = [0] * 10; + self.friend_robot_HP = [0] * 10 + self.enemy_robot_HP = [0] * 10 # Robot Status - self.robot_id = 0 # 机器人ID - self.current_hp = 0 # 当前血量 - self.maximum_hp = 0 # 最大血量 - self.shooter_heat_limit = 0 # 热量上限 - self.shooter_heat = 0 # 发射机构的枪口热量 + self.maximum_hp = 400 # 最大血量 + self.shooter_heat_limit = 400 # 热量上限 + + self.robot_id = None # 机器人ID + self.current_hp = None # 当前血量 + self.shooter_heat = None # 发射机构的枪口热量 # Friend Position self.friend_position = [[]] # RM Vision - self.enemy_dis = 0 + self.enemy_dis = None + + # RFID + self.rfid_patrol_status = None # Custom Variables - self.percent_hp = 0 - self.high_heat_flag = 0 + self.percent_hp = None + self.high_heat_flag = None def callback_AllRobotHP(self, msg): - if self.friend_color == 0: + if self.friend_color == TeamColor.RED: self.friend_robot_HP[1] = msg.red_1_robot_hp self.friend_robot_HP[2] = msg.red_2_robot_hp self.friend_robot_HP[3] = msg.red_3_robot_hp @@ -48,7 +58,7 @@ class CallbackMsg(Node): self.enemy_robot_HP[7] = msg.blue_7_robot_hp # 敌方哨兵血量 self.enemy_robot_HP[8] = msg.blue_outpost_hp self.enemy_robot_HP[9] = msg.blue_base_hp - elif self.friend_color == 1: + elif self.friend_color == TeamColor.BLUE: self.friend_robot_HP[1] = msg.blue_1_robot_hp self.friend_robot_HP[2] = msg.blue_2_robot_hp self.friend_robot_HP[3] = msg.blue_3_robot_hp @@ -82,8 +92,6 @@ class CallbackMsg(Node): def callback_RobotStatus(self, msg): self.robot_id = msg.robot_id self.current_hp = msg.current_hp - self.maximum_hp = msg.maximum_hp - self.shooter_heat_limit = msg.shooter_heat_limit self.shooter_heat = msg.shooter_heat self.percent_hp = self.current_hp / self.maximum_hp @@ -92,26 +100,28 @@ class CallbackMsg(Node): self.high_heat_flag = 1 else: self.high_heat_flag = 0 + ''' - 敌我识别 - TODO：根据24赛季新裁判系统协议，更改敌我判断的条件 + 根据哨兵机器人ID，判断我方阵营颜色 ''' if 1 <= self.robot_id <= 7: - self.friend_color = 0 + self.friend_color = TeamColor.RED # logging.get_logger('RobotStatus').info('We are: RED') elif 101 <= self.robot_id <= 107: - self.friend_color = 1 + self.friend_color = TeamColor.BLUE # logging.get_logger('RobotStatus').info('We are: BLUE') else: - logging.get_logger('RobotStatus').warn('Cannot identified Color') + logging.get_logger('RobotStatus').warn('Cannot identify team color') # DEBUG + # logging.get_logger('RobotStatus').info('We are: ' + str(self.friend_color.name)) # logging.get_logger('RobotStatus').info('self.robot_id: ' + str(self.robot_id)) # logging.get_logger('RobotStatus').info('self.current_hp: ' + str(self.current_hp)) - # logging.get_logger('RobotStatus').info('self.maximum_hp: ' + str(self.maximum_hp)) - # logging.get_logger('RobotStatus').info('self.shooter_heat_limit: ' + str(self.shooter_heat_limit)) # logging.get_logger('RobotStatus').info('self.shooter_heat: ' + str(self.shooter_heat)) # logging.get_logger('RobotStatus').info(' ------------------------------------- ') def callback_rm_vision(self, msg): - self.enemy_dis = msg.distance_to_image_center \ No newline at end of file + self.enemy_dis = msg.distance_to_image_center + + def callback_RFID(self, msg): + self.rfid_patrol_status = msg.rfid_patrol_status \ No newline at end of file diff --git a/src/rm_decision/rm_decision/pb_auto_fsm.py b/src/rm_decision/rm_decision/pb_auto_fsm.py index b8fcf5c70f0ab0d1208f21a75f479a8ba4a53f42..51a5890a1707af0f68e327d8ebfd543fbdb05d78 100644 --- a/src/rm_decision/rm_decision/pb_auto_fsm.py +++ b/src/rm_decision/rm_decision/pb_auto_fsm.py @@ -11,7 +11,7 @@ from rclpy.qos import QoSProfile from rclpy import qos from geometry_msgs.msg import Twist, PoseStamped -from rm_decision_interfaces.msg import RobotStatus, FriendLocation, AllRobotHP, Armor +from rm_decision_interfaces.msg import RobotStatus, FriendLocation, AllRobotHP, Armor, RFID from rm_decision.callback_msg import CallbackMsg from rm_decision.robot_navigator import BasicNavigator @@ -39,6 +39,7 @@ class AutoFSM(Node): self.nav_timeout = 10.0 self.interval_time = 1.0 self.follow_robot_id = None + self.out_patrol_time = 0 self.msg_callback = CallbackMsg() self.FSM_state = 2 @@ -59,11 +60,18 @@ class AutoFSM(Node): '/detector/armors', self.msg_callback.callback_rm_vision, qos.qos_profile_sensor_data) + self.RFID_sub = self.create_subscription(RFID, + '/rfid', + self.msg_callback.callback_RFID, + QoSProfile(depth=1)) - # Init function - Utility.yaml_read(self) + # Set rate self.rate = self.create_rate(0.5) self.rate_nav = self.create_rate(100) + self.rate_RFID = self.create_rate(1) + + # Init function + Utility.yaml_read(self) self.init_wait_for_messages() # Connect to Navigation @@ -72,6 +80,8 @@ class AutoFSM(Node): self.navigator.waitUntilNav2Active() self.navigator.get_logger().info('成功连接导航服务') + time.sleep(3) # 停滞三秒，让所有 msg 都被正常接收 + # Create Threads self.thread_FSM_ = threading.Thread(target=self.thread_FSM) self.thread_FSM_.start() @@ -79,16 +89,32 @@ class AutoFSM(Node): self.thread_FSM_state_ = threading.Thread(target=self.thread_FSM_state) self.thread_FSM_state_.start() + self.thread_RFID_ = threading.Thread(target=self.thread_RFID) + self.thread_RFID_.start() + def init_wait_for_messages(self): message_topics = [ (RobotStatus, '/robot_status'), (AllRobotHP, '/robot_hp'), (FriendLocation, '/friend_location'), - (Armor, '/detector/armors') + (Armor, '/detector/armors'), + (RFID, '/rfid') ] for message_type, topic in message_topics: self.get_logger().info(f'等待 {topic} 消息') Utility.wait_for_message(self, message_type, topic) + + def thread_RFID(self): + while rclpy.ok(): + if self.msg_callback.rfid_patrol_status == 0: + # Not detected patrol_RFID, the robot is not in the patrol area, timer counts in seconds + self.out_patrol_time += 1 + self.get_logger().info('out_patrol_time = ' + str(self.out_patrol_time)) + else: + # Detected patrol_RFID, reset the timer + self.get_logger().info('Detected patrol RFID') + self.out_patrol_time = 0 + self.rate_RFID.sleep() def thread_FSM_state(self): while rclpy.ok(): @@ -96,9 +122,9 @@ class AutoFSM(Node): self.rate_nav.sleep() def state_update(self): - if self.msg_callback.percent_hp < self.set_unhealth_point_percent or self.msg_callback.high_heat_flag == 1: + if self.msg_callback.percent_hp < self.set_unhealth_point_percent or self.msg_callback.high_heat_flag == 1 or self.out_patrol_time > 30: ''' - 血量百分比低于设定值 或 枪口热量过高，进入逃跑状态 + 血量百分比低于设定值 或 枪口热量过高 或 哨兵离开巡逻区过长，进入逃跑状态 ''' self.FSM_state = 4 @@ -146,7 +172,7 @@ class AutoFSM(Node): if reach_result == 1: self.get_logger().info("到达目标点 " + str(self.random_list[random_num])) reach_result = 0 - time.sleep(self.interval_time) + self.rate.sleep() self.get_logger().info("退出随机跑点") def mission_follow(self): diff --git a/src/rm_decision_interfaces/CMakeLists.txt b/src/rm_decision_interfaces/CMakeLists.txt index 04275c34c925146b14c87bac8c3ce24f1373be9b..110503b2beb0b8fd0f13018ea7c6c13d3970f51d 100644 --- a/src/rm_decision_interfaces/CMakeLists.txt +++ b/src/rm_decision_interfaces/CMakeLists.txt @@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/RobotStatus.msg" "msg/FriendLocation.msg" "msg/Armor.msg" + "msg/RFID.msg" DEPENDENCIES std_msgs diff --git a/src/rm_decision_interfaces/msg/AllRobotHP.msg b/src/rm_decision_interfaces/msg/AllRobotHP.msg index ba44966158c41d5c0a4d59aa21a7dc5aec99587c..da08b498dcbe017f477f7accb4f0b59fe942c033 100644 --- a/src/rm_decision_interfaces/msg/AllRobotHP.msg +++ b/src/rm_decision_interfaces/msg/AllRobotHP.msg @@ -1,3 +1,5 @@ +# v1.6 0x0003 机器人血量数据 + uint16 red_1_robot_hp uint16 red_2_robot_hp uint16 red_3_robot_hp diff --git a/src/rm_decision_interfaces/msg/FriendLocation.msg b/src/rm_decision_interfaces/msg/FriendLocation.msg index a672c8acea6c3889b3bd62bcd677ff8b0224e49e..1398497ad6a3f517268d5146834aaae7a4189679 100644 --- a/src/rm_decision_interfaces/msg/FriendLocation.msg +++ b/src/rm_decision_interfaces/msg/FriendLocation.msg @@ -1,3 +1,5 @@ +# v1.6 0x020B 地面机器人位置数据 + float64 hero_x float64 hero_y diff --git a/src/rm_decision_interfaces/msg/RFID.msg b/src/rm_decision_interfaces/msg/RFID.msg new file mode 100644 index 0000000000000000000000000000000000000000..6b13aed58db2d856f7d3c8acf35ac77a5ffa7ca1 --- /dev/null +++ b/src/rm_decision_interfaces/msg/RFID.msg @@ -0,0 +1,2 @@ +# v1.6 0x0209 机器人RFID状态（此处仅保留我方哨兵巡逻区） +bool rfid_patrol_status \ No newline at end of file diff --git a/src/rm_decision_interfaces/msg/RobotStatus.msg b/src/rm_decision_interfaces/msg/RobotStatus.msg index f42442bf71a8a06fa14de8a7740f5f6e1d35f28a..851fc865c3b41a442dc923306a7805bb1305d0cd 100644 --- a/src/rm_decision_interfaces/msg/RobotStatus.msg +++ b/src/rm_decision_interfaces/msg/RobotStatus.msg @@ -1,8 +1,6 @@ -# From 0x0201 +# v1.6 0x0201 机器人性能体系数据 uint8 robot_id # 机器人ID （1~7->红，101~107->蓝） -uint16 current_hp # 当前血量 -uint16 maximum_hp # 最大血量 -uint16 shooter_heat_limit # 热量上限 +uint16 current_hp # 血量 -# From 0x0202 -uint16 shooter_heat # 发射机构的枪口热量 +# v1.6 0x0202 实时功率热量数据 +uint16 shooter_heat # 枪口热量 diff --git a/src/rm_localization/FAST_LIO/config/mid360.yaml b/src/rm_localization/FAST_LIO/config/mid360.yaml index cc5760d3345c36b0a165c6121656b4133235f490..bee5a839e6c9ac27ea4090f54cc5a327dd580be8 100644 --- a/src/rm_localization/FAST_LIO/config/mid360.yaml +++ b/src/rm_localization/FAST_LIO/config/mid360.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: common: - lid_topic: "/mid360" - imu_topic: "/imu" + lid_topic: "/livox/lidar" + imu_topic: "/imu/data" time_sync_en: false # ONLY turn on when external time synchronization is really not possible time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 @@ -20,14 +20,13 @@ b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 360.0 - det_range: 200.0 + det_range: 100.0 extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.00, 0.00, -0.05 ] extrinsic_R: [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ] - publish: path_en: true # true: publish Path scan_publish_en: true # false: close all the point cloud output diff --git a/src/rm_localization/FAST_LIO/rviz_cfg/fastlio.rviz b/src/rm_localization/FAST_LIO/rviz_cfg/fastlio.rviz index 5550b795307984157d7c483b37caa953e2a154ca..467dd5c4342936d84685c013c6c276a8d6ceece0 100644 --- a/src/rm_localization/FAST_LIO/rviz_cfg/fastlio.rviz +++ b/src/rm_localization/FAST_LIO/rviz_cfg/fastlio.rviz @@ -26,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: CloudRegistered + SyncSource: CloudMap Visualization Manager: Class: "" Displays: @@ -35,9 +35,9 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - body: + livox_frame: Value: true - camera_init: + odom: Value: true Marker Scale: 1 Name: TF @@ -45,8 +45,8 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - camera_init: - body: + odom: + livox_frame: {} Update Interval: 0 Value: true @@ -188,8 +188,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 4.614229202270508 - Min Value: -1.0074758529663086 + Max Value: 2.9017436504364014 + Min Value: -2.2115612030029297 Value: true Axis: Z Channel Name: intensity @@ -222,7 +222,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 0; 0; 0 - Fixed Frame: camera_init + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -280,10 +280,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.454797625541687 + Pitch: 0.9397971630096436 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 4.193554878234863 + Yaw: 4.408558368682861 Saved: ~ Window Geometry: Displays: diff --git a/src/rm_localization/FAST_LIO/src/laserMapping.cpp b/src/rm_localization/FAST_LIO/src/laserMapping.cpp index d4585ec14aa7fd7b0f02cc2fc94be5f023c1069b..03c1232077c583d22614c21405a9772f86953773 100644 --- a/src/rm_localization/FAST_LIO/src/laserMapping.cpp +++ b/src/rm_localization/FAST_LIO/src/laserMapping.cpp @@ -507,7 +507,7 @@ void publish_frame_world(rclcpp::Publisher::Share pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); - laserCloudmsg.header.frame_id = "camera_init"; + laserCloudmsg.header.frame_id = "odom"; pubLaserCloudFull->publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } @@ -559,7 +559,7 @@ void publish_frame_body(rclcpp::Publisher::Shared sensor_msgs::msg::PointCloud2 laserCloudmsg; pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); - laserCloudmsg.header.frame_id = "body"; + laserCloudmsg.header.frame_id = "livox_frame"; pubLaserCloudFull_body->publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } @@ -576,7 +576,7 @@ void publish_effect_world(rclcpp::Publisher::Shar sensor_msgs::msg::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); laserCloudFullRes3.header.stamp = get_ros_time(lidar_end_time); - laserCloudFullRes3.header.frame_id = "camera_init"; + laserCloudFullRes3.header.frame_id = "odom"; pubLaserCloudEffect->publish(laserCloudFullRes3); } @@ -597,7 +597,7 @@ void publish_map(rclcpp::Publisher::SharedPtr pub sensor_msgs::msg::PointCloud2 laserCloudmsg; pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg); laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); - laserCloudmsg.header.frame_id = "camera_init"; + laserCloudmsg.header.frame_id = "odom"; pubLaserCloudMap->publish(laserCloudmsg); } @@ -622,8 +622,8 @@ void set_posestamp(T & out) void publish_odometry(const rclcpp::Publisher::SharedPtr pubOdomAftMapped, std::unique_ptr & tf_br) { - odomAftMapped.header.frame_id = "camera_init"; - odomAftMapped.child_frame_id = "body"; + odomAftMapped.header.frame_id = "odom"; + odomAftMapped.child_frame_id = "base_link"; odomAftMapped.header.stamp = get_ros_time(lidar_end_time); set_posestamp(odomAftMapped.pose); pubOdomAftMapped->publish(odomAftMapped); @@ -640,8 +640,9 @@ void publish_odometry(const rclcpp::Publisher::SharedPt } geometry_msgs::msg::TransformStamped trans; - trans.header.frame_id = "camera_init"; - trans.child_frame_id = "body"; + trans.header.frame_id = "odom"; + trans.child_frame_id = "base_link"; + trans.header.stamp = odomAftMapped.header.stamp; //ADD trans.transform.translation.x = odomAftMapped.pose.pose.position.x; trans.transform.translation.y = odomAftMapped.pose.pose.position.y; trans.transform.translation.z = odomAftMapped.pose.pose.position.z; @@ -656,7 +657,7 @@ void publish_path(rclcpp::Publisher::SharedPtr pubPath) { set_posestamp(msg_body_pose); msg_body_pose.header.stamp = get_ros_time(lidar_end_time); // ros::Time().fromSec(lidar_end_time); - msg_body_pose.header.frame_id = "camera_init"; + msg_body_pose.header.frame_id = "odom"; /*** if path is too large, the rvis will crash ***/ static int jjj = 0; @@ -893,7 +894,7 @@ public: RCLCPP_INFO(this->get_logger(), "p_pre->lidar_type %d", p_pre->lidar_type); path.header.stamp = this->get_clock()->now(); - path.header.frame_id ="camera_init"; + path.header.frame_id ="odom"; FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); diff --git a/src/rm_localization/icp_localization_ros2/config/node_params.yaml b/src/rm_localization/icp_localization_ros2/config/node_params.yaml index 6a4703800cc9442a8db2d917afa0a106e93ae28a..ae1e2c3ad0fc4d0b1af2ab512152144ae2c4af97 100644 --- a/src/rm_localization/icp_localization_ros2/config/node_params.yaml +++ b/src/rm_localization/icp_localization_ros2/config/node_params.yaml @@ -4,11 +4,11 @@ icp_localization_ros2: range_data_topic: "/cloud_registered_body" num_accumulated_range_data: 1 - imu_data_topic: "/imu" - odometry_data_topic: "/odom" + imu_data_topic: "/imu/data" + odometry_data_topic: "/Odometry" # Utilize '/Odometry' from FAST_LIO when in REAL environment is_use_odometry: true - is_provide_odom_frame: false - gravity_vector_filter_time_constant: 0.05 + is_provide_odom_frame: true + gravity_vector_filter_time_constant: 0.01 fixed_frame: "map" min_num_odom_msgs_before_ready: 10 @@ -30,8 +30,8 @@ yaw: 0.0 odometry_source_to_range_sensor: - x: 0.000 - y: 0.000 + x: 0.0 + y: 0.0 z: 0.0 roll: 0.0 pitch: 0.0 diff --git a/src/rm_navigation/params/mapper_params_online_async.yaml b/src/rm_navigation/params/mapper_params_online_async.yaml index 0c63d22ac5f4b820ea1ee9d2e8caf26a56067966..f1677b6d2dfd12b30b98c09e38e353efe5e595d3 100644 --- a/src/rm_navigation/params/mapper_params_online_async.yaml +++ b/src/rm_navigation/params/mapper_params_online_async.yaml @@ -12,7 +12,7 @@ slam_toolbox: # ROS Parameters odom_frame: odom map_frame: map - base_frame: livox + base_frame: livox_frame scan_topic: /scan mode: mapping #localization @@ -70,4 +70,4 @@ slam_toolbox: coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 - use_response_expansion: true + use_response_expansion: true \ No newline at end of file diff --git a/src/rm_navigation/params/nav2_params.yaml b/src/rm_navigation/params/nav2_params.yaml index 280aa96d605e61ce761376cba14b05b0dc56fef1..2630e4adefc4dae9eb0dca2c48e3a062331cf5a3 100644 --- a/src/rm_navigation/params/nav2_params.yaml +++ b/src/rm_navigation/params/nav2_params.yaml @@ -44,7 +44,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: /Odometry bt_loop_duration: 10 default_server_timeout: 20 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -136,86 +136,84 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.8 - # PurePursuit + # PurePursuit + # https://navigation.ros.org/configuration/packages/configuring-regulated-pp.html FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - # https://navigation.ros.org/configuration/packages/configuring-regulated-pp.html desired_linear_vel: 2.5 lookahead_dist: 0.6 min_lookahead_dist: 0.3 max_lookahead_dist: 0.9 lookahead_time: 1.5 - rotate_to_heading_angular_vel: 20.0 + rotate_to_heading_angular_vel: 10.0 transform_tolerance: 0.1 - use_velocity_scaled_lookahead_dist: false + use_velocity_scaled_lookahead_dist: true min_approach_linear_velocity: 0.05 - approach_velocity_scaling_dist: 0.6 - use_collision_detection: true + approach_velocity_scaling_dist: 2.0 + use_collision_detection: false max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_fixed_curvature_lookahead: false - curvature_lookahead_dist: 0.6 - use_cost_regulated_linear_velocity_scaling: false - regulated_linear_scaling_min_radius: 0.25 + curvature_lookahead_dist: 2.0 + use_cost_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.15 regulated_linear_scaling_min_speed: 0.5 use_rotate_to_heading: true allow_reversing: false rotate_to_heading_min_angle: 0.785 - max_angular_accel: 20.0 + max_angular_accel: 15.0 max_robot_pose_search_dist: 10.0 use_interpolation: true # DWB parameters - # FollowPath: - # plugin: "dwb_core::DWBLocalPlanner" - # debug_trajectory_details: True - # min_vel_x: -3.0 - # min_vel_y: -2.0 - # max_vel_x: 3.0 - # max_vel_y: 2.0 - # max_vel_theta: 10.0 - # min_speed_xy: 0.0 - # max_speed_xy: 3.0 - # min_speed_theta: 0.0 - # # Add high threshold velocity for turtlebot 3 issue. - # # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - # acc_lim_x: 2.5 - # acc_lim_y: 2.0 - # acc_lim_theta: 5.0 - # decel_lim_x: -2.5 - # decel_lim_y: -2.0 - # decel_lim_theta: -3.2 - # vx_samples: 20 - # vy_samples: 5 - # vtheta_samples: 20 - # sim_time: 1.7 - # linear_granularity: 0.05 - # angular_granularity: 0.025 - # transform_tolerance: 0.2 - # xy_goal_tolerance: 0.25 - # trans_stopped_velocity: 0.25 - # short_circuit_trajectory_evaluation: True - # stateful: True - # critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - # BaseObstacle.scale: 0.02 - # PathAlign.scale: 32.0 - # PathAlign.forward_point_distance: 0.1 - # GoalAlign.scale: 24.0 - # GoalAlign.forward_point_distance: 0.1 - # PathDist.scale: 32.0 - # GoalDist.scale: 24.0 - # RotateToGoal.scale: 32.0 - # RotateToGoal.slowing_factor: 5.0 - # RotateToGoal.lookahead_time: -1.0 + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: -2.5 + min_vel_y: -2.5 + max_vel_x: 2.5 + max_vel_y: 2.5 + max_vel_theta: 12.0 + min_speed_xy: -8.0 + max_speed_xy: 8.0 + min_speed_theta: -12.0 + acc_lim_x: 5.0 + acc_lim_y: 5.0 + acc_lim_theta: 15.0 + decel_lim_x: -5.0 + decel_lim_y: -5.0 + decel_lim_theta: -15.0 + vx_samples: 20 + vy_samples: 20 + vtheta_samples: 20 + sim_time: 0.16 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + GoalAlign.scale: 24.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 # Recovery - shrink_horizon_backup: True - shrink_horizon_min_duration: 10.0 - oscillation_recovery: True - oscillation_v_eps: 0.1 - oscillation_omega_eps: 0.1 - oscillation_recovery_mimaxn_duration: 10.0 - oscillation_filter_duration: 10.0 + # shrink_horizon_backup: True + # shrink_horizon_min_duration: 10.0 + # oscillation_recovery: True + # oscillation_v_eps: 0.1 + # oscillation_omega_eps: 0.1 + # oscillation_recovery_mimaxn_duration: 10.0 + # oscillation_filter_duration: 10.0 local_costmap: @@ -399,11 +397,11 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: True feedback: "OPEN_LOOP" - max_velocity: [2.0, 5.0, 12.0] - min_velocity: [-2.0, -5.0, -12.0] - max_accel: [5.5, 3.0, 15.2] - max_decel: [-5.5, -3.0, -15.2] - odom_topic: "odom" + max_velocity: [2.5, 2.5, 12.0] + min_velocity: [-2.5, -2.5, -12.0] + max_accel: [5.0, 5.0, 15.0] + max_decel: [-5.0, -5.0, -15.0] + odom_topic: "Odometry" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 diff --git a/src/rm_perception/imu_complementary_filter/CHANGELOG.rst b/src/rm_perception/imu_complementary_filter/CHANGELOG.rst new file mode 100644 index 0000000000000000000000000000000000000000..d2aede662b696c7f6e1c6edda5765856db49e799 --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/CHANGELOG.rst @@ -0,0 +1,128 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package imu_complementary_filter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.1.3 (2022-12-07) +------------------ +* complementary: Build shared library + See `#172 `_. +* Update CMakeLists to use targets +* Remove node\_ prefix. (`#163 `_) +* Contributors: Martin Günther, Max Polzin + +2.1.2 (2022-07-14) +------------------ + +2.1.1 (2022-05-24) +------------------ +* Add missing build dependency to package.xml. (`#161 `_) +* Contributors: Martin Günther, Steven! Ragnarök + +2.1.0 (2022-05-02) +------------------ +* complementary: Add missing dependency on geometry_msgs +* Contributors: Martin Günther + +2.0.0 (2022-04-12) +------------------ +* Initial release into ROS2 foxy, galactic and rolling +* Fix gcc warnings + clang-tidy suggestions +* Fix CMakeLists +* Reformat python code using black +* Manually reformat licenses + defines +* Reformat everything using clang-format +* Fix trailing whitespace +* Add launch directory to CMakeLists.txt (`#146 `_) +* Port imu_complementary_filter to ROS2 (`#138 `_) +* Madgwick for eloquent (`#110 `_) +* Contributors: Guido Sanchez, Martin Günther, Maximilian Schik, tgreier + +1.2.2 (2020-05-25) +------------------ +* fix install path & boost linkage issues +* Contributors: Martin Günther, Sean Yen + +1.2.1 (2019-05-06) +------------------ +* Remove junk xml (`#93 `_) +* Fix C++14 builds (`#89 `_) +* Contributors: David V. Lu!!, Paul Bovbel + +1.2.0 (2018-05-25) +------------------ +* Add std dev parameter to orientation estimate from filter (`#85 `_) + Similar to `#41 `_, but not using dynamic_reconfigure as not implemented for complementary filter +* Contributors: Stefan Kohlbrecher + +1.1.5 (2017-05-24) +------------------ + +1.1.4 (2017-05-22) +------------------ + +1.1.3 (2017-03-10) +------------------ +* complementary_filter: move const initializations out of header + Initialization of static consts other than int (here: float) inside the + class declaration is not permitted in C++. It works in gcc (due to a + non-standard extension), but throws an error in C++11. +* Contributors: Martin Guenther + +1.1.2 (2016-09-07) +------------------ + +1.1.1 (2016-09-07) +------------------ + +1.1.0 (2016-04-25) +------------------ + +1.0.11 (2016-04-22) +------------------- + +1.0.10 (2016-04-22) +------------------- +* Remove Eigen dependency + Eigen is not actually used anywhere. Thanks @asimay! +* Removed main function from shared library +* Contributors: Martin Guenther, Matthias Nieuwenhuisen + +1.0.9 (2015-10-16) +------------------ +* complementary: Add Eigen dependency + Fixes `#54 `_. +* Contributors: Martin Günther + +1.0.8 (2015-10-07) +------------------ + +1.0.7 (2015-10-07) +------------------ +* Allow remapping imu namespace +* Publish RPY as Vector3Stamped +* Add params: constant_dt, publish_tf, reverse_tf, publish_debug_topics +* Use MagneticField instead of Vector3 +* Contributors: Martin Günther + +1.0.6 (2015-10-06) +------------------ +* Add new package: imu_complementary_filter +* Contributors: Roberto G. Valentini, Martin Günther, Michael Görner + +1.0.5 (2015-06-24) +------------------ + +1.0.4 (2015-05-06) +------------------ + +1.0.3 (2015-01-29) +------------------ + +1.0.2 (2015-01-27) +------------------ + +1.0.1 (2014-12-10) +------------------ + +1.0.0 (2014-11-28) +------------------ diff --git a/src/rm_perception/imu_complementary_filter/CMakeLists.txt b/src/rm_perception/imu_complementary_filter/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..79aa917cfc0882cf472516a04c67a81a5997ac9a --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(imu_complementary_filter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +add_compile_options(-std=c++17 -O3) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +add_library(complementary_filter SHARED + src/complementary_filter.cpp + src/complementary_filter_ros.cpp + include/imu_complementary_filter/complementary_filter.h + include/imu_complementary_filter/complementary_filter_ros.h +) +target_compile_features(complementary_filter PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(complementary_filter PUBLIC + $ + $) +ament_target_dependencies(complementary_filter + geometry_msgs + message_filters + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(complementary_filter PRIVATE "IMU_COMPLEMENTARY_FILTER_BUILDING_LIBRARY") + +# create complementary_filter_node executable +add_executable(complementary_filter_node + src/complementary_filter_node.cpp) +target_include_directories(complementary_filter_node PUBLIC + $ + $) +target_link_libraries(complementary_filter_node complementary_filter) + +install( + TARGETS complementary_filter + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS + complementary_filter_node + DESTINATION lib/${PROJECT_NAME} +) + +## Mark cpp header files for installation +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_libraries(complementary_filter) +ament_export_targets( + export_${PROJECT_NAME} +) +ament_package() diff --git a/src/rm_perception/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h b/src/rm_perception/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h new file mode 100644 index 0000000000000000000000000000000000000000..4758cad2e3a7905a20e368f556e47067640751a0 --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h @@ -0,0 +1,175 @@ +/* + @author Roberto G. Valenti + + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + + 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 the copyright holder 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_H +#define IMU_TOOLS_COMPLEMENTARY_FILTER_H + +namespace imu_tools { + +class ComplementaryFilter +{ + public: + ComplementaryFilter(); + virtual ~ComplementaryFilter(); + + bool setGainAcc(double gain); + bool setGainMag(double gain); + double getGainAcc() const; + double getGainMag() const; + + bool setBiasAlpha(double bias_alpha); + double getBiasAlpha() const; + + // When the filter is in the steady state, bias estimation will occur (if + // the parameter is enabled). + bool getSteadyState() const; + + void setDoBiasEstimation(bool do_bias_estimation); + bool getDoBiasEstimation() const; + + void setDoAdaptiveGain(bool do_adaptive_gain); + bool getDoAdaptiveGain() const; + + double getAngularVelocityBiasX() const; + double getAngularVelocityBiasY() const; + double getAngularVelocityBiasZ() const; + + // Set the orientation, as a Hamilton Quaternion, of the body frame wrt the + // fixed frame. + void setOrientation(double q0, double q1, double q2, double q3); + + // Get the orientation, as a Hamilton Quaternion, of the body frame wrt the + // fixed frame. + void getOrientation(double& q0, double& q1, double& q2, double& q3) const; + + // Update from accelerometer and gyroscope data. + // [ax, ay, az]: Normalized gravity vector. + // [wx, wy, wz]: Angular veloctiy, in rad / s. + // dt: time delta, in seconds. + void update(double ax, double ay, double az, double wx, double wy, + double wz, double dt); + + // Update from accelerometer, gyroscope, and magnetometer data. + // [ax, ay, az]: Normalized gravity vector. + // [wx, wy, wz]: Angular veloctiy, in rad / s. + // [mx, my, mz]: Magnetic field, units irrelevant. + // dt: time delta, in seconds. + void update(double ax, double ay, double az, double wx, double wy, + double wz, double mx, double my, double mz, double dt); + + private: + static const double kGravity; + static const double gamma_; + // Bias estimation steady state thresholds + static const double kAngularVelocityThreshold; + static const double kAccelerationThreshold; + static const double kDeltaAngularVelocityThreshold; + + // Gain parameter for the complementary filter, belongs in [0, 1]. + double gain_acc_; + double gain_mag_; + + // Bias estimation gain parameter, belongs in [0, 1]. + double bias_alpha_; + + // Parameter whether to do bias estimation or not. + bool do_bias_estimation_; + + // Parameter whether to do adaptive gain or not. + bool do_adaptive_gain_; + + bool initialized_; + bool steady_state_; + + // The orientation as a Hamilton quaternion (q0 is the scalar). Represents + // the orientation of the fixed frame wrt the body frame. + double q0_, q1_, q2_, q3_; + + // Bias in angular velocities; + double wx_prev_, wy_prev_, wz_prev_; + + // Bias in angular velocities; + double wx_bias_, wy_bias_, wz_bias_; + + void updateBiases(double ax, double ay, double az, double wx, double wy, + double wz); + + bool checkState(double ax, double ay, double az, double wx, double wy, + double wz) const; + + void getPrediction(double wx, double wy, double wz, double dt, + double& q0_pred, double& q1_pred, double& q2_pred, + double& q3_pred) const; + + void getMeasurement(double ax, double ay, double az, double& q0_meas, + double& q1_meas, double& q2_meas, double& q3_meas); + + void getMeasurement(double ax, double ay, double az, double mx, double my, + double mz, double& q0_meas, double& q1_meas, + double& q2_meas, double& q3_meas); + + void getAccCorrection(double ax, double ay, double az, double p0, double p1, + double p2, double p3, double& dq0, double& dq1, + double& dq2, double& dq3); + + void getMagCorrection(double mx, double my, double mz, double p0, double p1, + double p2, double p3, double& dq0, double& dq1, + double& dq2, double& dq3); + + double getAdaptiveGain(double alpha, double ax, double ay, double az); +}; + +// Utility math functions: + +void normalizeVector(double& x, double& y, double& z); + +void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3); + +void scaleQuaternion(double gain, double& dq0, double& dq1, double& dq2, + double& dq3); + +void invertQuaternion(double q0, double q1, double q2, double q3, + double& q0_inv, double& q1_inv, double& q2_inv, + double& q3_inv); + +void quaternionMultiplication(double p0, double p1, double p2, double p3, + double q0, double q1, double q2, double q3, + double& r0, double& r1, double& r2, double& r3); + +void rotateVectorByQuaternion(double x, double y, double z, double q0, + double q1, double q2, double q3, double& vx, + double& vy, double& vz); + +} // namespace imu_tools + +#endif // IMU_TOOLS_COMPLEMENTARY_FILTER_H diff --git a/src/rm_perception/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h b/src/rm_perception/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h new file mode 100644 index 0000000000000000000000000000000000000000..9c9a44f5a317d8b1c1858a16332e8983a725c503 --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h @@ -0,0 +1,107 @@ +/* + @author Roberto G. Valenti + + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + + 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 the copyright holder 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H +#define IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "imu_complementary_filter/complementary_filter.h" + +namespace imu_tools { + +class ComplementaryFilterROS : public rclcpp::Node +{ + public: + ComplementaryFilterROS(); + ~ComplementaryFilterROS() override; + + private: + // Convenience typedefs + typedef sensor_msgs::msg::Imu ImuMsg; + typedef sensor_msgs::msg::MagneticField MagMsg; + typedef geometry_msgs::msg::Vector3Stamped RpyVectorMsg; + typedef message_filters::sync_policies::ApproximateTime + SyncPolicy; + typedef message_filters::Synchronizer Synchronizer; + typedef message_filters::Subscriber ImuSubscriber; + typedef message_filters::Subscriber MagSubscriber; + + // ROS-related variables. + std::shared_ptr imu_subscriber_; + std::shared_ptr mag_subscriber_; + std::shared_ptr sync_; + + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::Publisher::SharedPtr rpy_publisher_; + rclcpp::Publisher::SharedPtr state_publisher_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + + // Parameters: + bool use_mag_{}; + bool publish_tf_{}; + bool reverse_tf_{}; + double constant_dt_{}; + bool publish_debug_topics_{}; + std::string fixed_frame_; + double orientation_variance_{}; + + // State variables: + ComplementaryFilter filter_; + rclcpp::Time time_prev_; + bool initialized_filter_; + + void initializeParams(); + void imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw); + void imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, + MagMsg::ConstSharedPtr mav_msg); + void publish(ImuMsg::ConstSharedPtr imu_msg_raw); + + tf2::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2, + double q3) const; +}; + +} // namespace imu_tools + +#endif // IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H diff --git a/src/rm_perception/imu_complementary_filter/launch/complementary_filter.launch.py b/src/rm_perception/imu_complementary_filter/launch/complementary_filter.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..8a10720998b9ce49308f40083c2b0ea90b555328 --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/launch/complementary_filter.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + Node( + package='imu_complementary_filter', + executable='complementary_filter_node', + name='complementary_filter_gain_node', + output='screen', + parameters=[ + {'do_bias_estimation': True}, + {'do_adaptive_gain': True}, + {'use_mag': False}, + {'gain_acc': 0.01}, + {'gain_mag': 0.01}, + ], + remappings=[ + ('/imu/data_raw', '/livox/imu'), + ] + ) + ] + ) diff --git a/src/rm_perception/imu_complementary_filter/package.xml b/src/rm_perception/imu_complementary_filter/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..d3c79511fe851f2f2c2cdb0796b19ac8ef91ecaa --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/package.xml @@ -0,0 +1,24 @@ + + + imu_complementary_filter + 2.1.3 + Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into a quaternion to represent the orientation of the device wrt the global frame. Based on the algorithm by Roberto G. Valenti etal. described in the paper "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" available at http://www.mdpi.com/1424-8220/15/8/19302 . + + Martin Günther + BSD + + http://www.mdpi.com/1424-8220/15/8/19302 + Roberto G. Valenti + + ament_cmake + geometry_msgs + message_filters + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros + + ament_cmake + + diff --git a/src/rm_perception/imu_complementary_filter/src/complementary_filter.cpp b/src/rm_perception/imu_complementary_filter/src/complementary_filter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1581ed91fd03416bb58380e60e43659cb36190e6 --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/src/complementary_filter.cpp @@ -0,0 +1,535 @@ +/* + @author Roberto G. Valenti + + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + + 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 the copyright holder 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#include "imu_complementary_filter/complementary_filter.h" + +#include +#include +#include + +namespace imu_tools { + +const double ComplementaryFilter::kGravity = 9.81; +const double ComplementaryFilter::gamma_ = 0.01; +// Bias estimation steady state thresholds +const double ComplementaryFilter::kAngularVelocityThreshold = 0.2; +const double ComplementaryFilter::kAccelerationThreshold = 0.1; +const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01; + +ComplementaryFilter::ComplementaryFilter() + : gain_acc_(0.01), + gain_mag_(0.01), + bias_alpha_(0.01), + do_bias_estimation_(true), + do_adaptive_gain_(false), + initialized_(false), + steady_state_(false), + q0_(1), + q1_(0), + q2_(0), + q3_(0), + wx_prev_(0), + wy_prev_(0), + wz_prev_(0), + wx_bias_(0), + wy_bias_(0), + wz_bias_(0) +{ +} + +ComplementaryFilter::~ComplementaryFilter() = default; + +void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation) +{ + do_bias_estimation_ = do_bias_estimation; +} + +bool ComplementaryFilter::getDoBiasEstimation() const +{ + return do_bias_estimation_; +} + +void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain) +{ + do_adaptive_gain_ = do_adaptive_gain; +} + +bool ComplementaryFilter::getDoAdaptiveGain() const +{ + return do_adaptive_gain_; +} + +bool ComplementaryFilter::setGainAcc(double gain) +{ + if (gain >= 0 && gain <= 1.0) + { + gain_acc_ = gain; + return true; + } else + return false; +} +bool ComplementaryFilter::setGainMag(double gain) +{ + if (gain >= 0 && gain <= 1.0) + { + gain_mag_ = gain; + return true; + } else + return false; +} + +double ComplementaryFilter::getGainAcc() const +{ + return gain_acc_; +} + +double ComplementaryFilter::getGainMag() const +{ + return gain_mag_; +} + +bool ComplementaryFilter::getSteadyState() const +{ + return steady_state_; +} + +bool ComplementaryFilter::setBiasAlpha(double bias_alpha) +{ + if (bias_alpha >= 0 && bias_alpha <= 1.0) + { + bias_alpha_ = bias_alpha; + return true; + } else + return false; +} + +double ComplementaryFilter::getBiasAlpha() const +{ + return bias_alpha_; +} + +void ComplementaryFilter::setOrientation(double q0, double q1, double q2, + double q3) +{ + // Set the state to inverse (state is fixed wrt body). + invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_); +} + +double ComplementaryFilter::getAngularVelocityBiasX() const +{ + return wx_bias_; +} + +double ComplementaryFilter::getAngularVelocityBiasY() const +{ + return wy_bias_; +} + +double ComplementaryFilter::getAngularVelocityBiasZ() const +{ + return wz_bias_; +} + +void ComplementaryFilter::update(double ax, double ay, double az, double wx, + double wy, double wz, double dt) +{ + if (!initialized_) + { + // First time - ignore prediction: + getMeasurement(ax, ay, az, q0_, q1_, q2_, q3_); + initialized_ = true; + return; + } + + // Bias estimation. + if (do_bias_estimation_) updateBiases(ax, ay, az, wx, wy, wz); + + // Prediction. + double q0_pred, q1_pred, q2_pred, q3_pred; + getPrediction(wx, wy, wz, dt, q0_pred, q1_pred, q2_pred, q3_pred); + + // Correction (from acc): + // q_ = q_pred * [(1-gain) * qI + gain * dq_acc] + // where qI = identity quaternion + double dq0_acc, dq1_acc, dq2_acc, dq3_acc; + getAccCorrection(ax, ay, az, q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc, + dq1_acc, dq2_acc, dq3_acc); + + double gain; + if (do_adaptive_gain_) + { + gain = getAdaptiveGain(gain_acc_, ax, ay, az); + + } else + { + gain = gain_acc_; + } + + scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc); + + quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc, + dq1_acc, dq2_acc, dq3_acc, q0_, q1_, q2_, q3_); + + normalizeQuaternion(q0_, q1_, q2_, q3_); +} + +void ComplementaryFilter::update(double ax, double ay, double az, double wx, + double wy, double wz, double mx, double my, + double mz, double dt) +{ + if (!initialized_) + { + // First time - ignore prediction: + getMeasurement(ax, ay, az, mx, my, mz, q0_, q1_, q2_, q3_); + initialized_ = true; + return; + } + + // Bias estimation. + if (do_bias_estimation_) updateBiases(ax, ay, az, wx, wy, wz); + + // Prediction. + double q0_pred, q1_pred, q2_pred, q3_pred; + getPrediction(wx, wy, wz, dt, q0_pred, q1_pred, q2_pred, q3_pred); + + // Correction (from acc): + // q_temp = q_pred * [(1-gain) * qI + gain * dq_acc] + // where qI = identity quaternion + double dq0_acc, dq1_acc, dq2_acc, dq3_acc; + getAccCorrection(ax, ay, az, q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc, + dq1_acc, dq2_acc, dq3_acc); + double alpha = gain_acc_; + if (do_adaptive_gain_) alpha = getAdaptiveGain(gain_acc_, ax, ay, az); + scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc); + + double q0_temp, q1_temp, q2_temp, q3_temp; + quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc, + dq1_acc, dq2_acc, dq3_acc, q0_temp, q1_temp, + q2_temp, q3_temp); + + // Correction (from mag): + // q_ = q_temp * [(1-gain) * qI + gain * dq_mag] + // where qI = identity quaternion + double dq0_mag, dq1_mag, dq2_mag, dq3_mag; + getMagCorrection(mx, my, mz, q0_temp, q1_temp, q2_temp, q3_temp, dq0_mag, + dq1_mag, dq2_mag, dq3_mag); + + scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag); + + quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp, dq0_mag, + dq1_mag, dq2_mag, dq3_mag, q0_, q1_, q2_, q3_); + + normalizeQuaternion(q0_, q1_, q2_, q3_); +} + +bool ComplementaryFilter::checkState(double ax, double ay, double az, double wx, + double wy, double wz) const +{ + double acc_magnitude = sqrt(ax * ax + ay * ay + az * az); + if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold) return false; + + if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold || + fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold || + fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold) + return false; + + if (fabs(wx - wx_bias_) > kAngularVelocityThreshold || + fabs(wy - wy_bias_) > kAngularVelocityThreshold || + fabs(wz - wz_bias_) > kAngularVelocityThreshold) + return false; + + return true; +} + +void ComplementaryFilter::updateBiases(double ax, double ay, double az, + double wx, double wy, double wz) +{ + steady_state_ = checkState(ax, ay, az, wx, wy, wz); + + if (steady_state_) + { + wx_bias_ += bias_alpha_ * (wx - wx_bias_); + wy_bias_ += bias_alpha_ * (wy - wy_bias_); + wz_bias_ += bias_alpha_ * (wz - wz_bias_); + } + + wx_prev_ = wx; + wy_prev_ = wy; + wz_prev_ = wz; +} + +void ComplementaryFilter::getPrediction(double wx, double wy, double wz, + double dt, double& q0_pred, + double& q1_pred, double& q2_pred, + double& q3_pred) const +{ + double wx_unb = wx - wx_bias_; + double wy_unb = wy - wy_bias_; + double wz_unb = wz - wz_bias_; + + q0_pred = q0_ + 0.5 * dt * (wx_unb * q1_ + wy_unb * q2_ + wz_unb * q3_); + q1_pred = q1_ + 0.5 * dt * (-wx_unb * q0_ - wy_unb * q3_ + wz_unb * q2_); + q2_pred = q2_ + 0.5 * dt * (wx_unb * q3_ - wy_unb * q0_ - wz_unb * q1_); + q3_pred = q3_ + 0.5 * dt * (-wx_unb * q2_ + wy_unb * q1_ - wz_unb * q0_); + + normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred); +} + +void ComplementaryFilter::getMeasurement(double ax, double ay, double az, + double mx, double my, double mz, + double& q0_meas, double& q1_meas, + double& q2_meas, double& q3_meas) +{ + // q_acc is the quaternion obtained from the acceleration vector + // representing the orientation of the Global frame wrt the Local frame with + // arbitrary yaw (intermediary frame). q3_acc is defined as 0. + double q0_acc, q1_acc, q2_acc, q3_acc; + + // Normalize acceleration vector. + normalizeVector(ax, ay, az); + if (az >= 0) + { + q0_acc = sqrt((az + 1) * 0.5); + q1_acc = -ay / (2.0 * q0_acc); + q2_acc = ax / (2.0 * q0_acc); + q3_acc = 0; + } else + { + double X = sqrt((1 - az) * 0.5); + q0_acc = -ay / (2.0 * X); + q1_acc = X; + q2_acc = 0; + q3_acc = ax / (2.0 * X); + } + + // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary + // frame by the inverse of q_acc. + // l = R(q_acc)^-1 m + double lx = (q0_acc * q0_acc + q1_acc * q1_acc - q2_acc * q2_acc) * mx + + 2.0 * (q1_acc * q2_acc) * my - 2.0 * (q0_acc * q2_acc) * mz; + double ly = 2.0 * (q1_acc * q2_acc) * mx + + (q0_acc * q0_acc - q1_acc * q1_acc + q2_acc * q2_acc) * my + + 2.0 * (q0_acc * q1_acc) * mz; + + // q_mag is the quaternion that rotates the Global frame (North West Up) + // into the intermediary frame. q1_mag and q2_mag are defined as 0. + double gamma = lx * lx + ly * ly; + double beta = sqrt(gamma + lx * sqrt(gamma)); + double q0_mag = beta / (sqrt(2.0 * gamma)); + double q3_mag = ly / (sqrt(2.0) * beta); + + // The quaternion multiplication between q_acc and q_mag represents the + // quaternion, orientation of the Global frame wrt the local frame. + // q = q_acc times q_mag + quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, q0_mag, 0, 0, + q3_mag, q0_meas, q1_meas, q2_meas, q3_meas); + // q0_meas = q0_acc*q0_mag; + // q1_meas = q1_acc*q0_mag + q2_acc*q3_mag; + // q2_meas = q2_acc*q0_mag - q1_acc*q3_mag; + // q3_meas = q0_acc*q3_mag; +} + +void ComplementaryFilter::getMeasurement(double ax, double ay, double az, + double& q0_meas, double& q1_meas, + double& q2_meas, double& q3_meas) +{ + // q_acc is the quaternion obtained from the acceleration vector + // representing the orientation of the Global frame wrt the Local frame with + // arbitrary yaw (intermediary frame). q3_acc is defined as 0. + + // Normalize acceleration vector. + normalizeVector(ax, ay, az); + + if (az >= 0) + { + q0_meas = sqrt((az + 1) * 0.5); + q1_meas = -ay / (2.0 * q0_meas); + q2_meas = ax / (2.0 * q0_meas); + q3_meas = 0; + } else + { + double X = sqrt((1 - az) * 0.5); + q0_meas = -ay / (2.0 * X); + q1_meas = X; + q2_meas = 0; + q3_meas = ax / (2.0 * X); + } +} + +void ComplementaryFilter::getAccCorrection(double ax, double ay, double az, + double p0, double p1, double p2, + double p3, double& dq0, double& dq1, + double& dq2, double& dq3) +{ + // Normalize acceleration vector. + normalizeVector(ax, ay, az); + + // Acceleration reading rotated into the world frame by the inverse + // predicted quaternion (predicted gravity): + double gx, gy, gz; + rotateVectorByQuaternion(ax, ay, az, p0, -p1, -p2, -p3, gx, gy, gz); + + // Delta quaternion that rotates the predicted gravity into the real + // gravity: + dq0 = sqrt((gz + 1) * 0.5); + dq1 = -gy / (2.0 * dq0); + dq2 = gx / (2.0 * dq0); + dq3 = 0.0; +} + +void ComplementaryFilter::getMagCorrection(double mx, double my, double mz, + double p0, double p1, double p2, + double p3, double& dq0, double& dq1, + double& dq2, double& dq3) +{ + // Magnetic reading rotated into the world frame by the inverse predicted + // quaternion: + double lx, ly, lz; + rotateVectorByQuaternion(mx, my, mz, p0, -p1, -p2, -p3, lx, ly, lz); + + // Delta quaternion that rotates the l so that it lies in the xz-plane + // (points north): + double gamma = lx * lx + ly * ly; + double beta = sqrt(gamma + lx * sqrt(gamma)); + dq0 = beta / (sqrt(2.0 * gamma)); + dq1 = 0.0; + dq2 = 0.0; + dq3 = ly / (sqrt(2.0) * beta); +} + +void ComplementaryFilter::getOrientation(double& q0, double& q1, double& q2, + double& q3) const +{ + // Return the inverse of the state (state is fixed wrt body). + invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3); +} + +double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, + double az) +{ + double a_mag = sqrt(ax * ax + ay * ay + az * az); + double error = fabs(a_mag - kGravity) / kGravity; + double factor; + double error1 = 0.1; + double error2 = 0.2; + double m = 1.0 / (error1 - error2); + double b = 1.0 - m * error1; + if (error < error1) + factor = 1.0; + else if (error < error2) + factor = m * error + b; + else + factor = 0.0; + // printf("FACTOR: %f \n", factor); + return factor * alpha; +} + +void normalizeVector(double& x, double& y, double& z) +{ + double norm = sqrt(x * x + y * y + z * z); + + x /= norm; + y /= norm; + z /= norm; +} + +void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3) +{ + double norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 /= norm; + q1 /= norm; + q2 /= norm; + q3 /= norm; +} + +void invertQuaternion(double q0, double q1, double q2, double q3, + double& q0_inv, double& q1_inv, double& q2_inv, + double& q3_inv) +{ + // Assumes quaternion is normalized. + q0_inv = q0; + q1_inv = -q1; + q2_inv = -q2; + q3_inv = -q3; +} + +void scaleQuaternion(double gain, double& dq0, double& dq1, double& dq2, + double& dq3) +{ + if (dq0 < 0.0) // 0.9 + { + // Slerp (Spherical linear interpolation): + double angle = acos(dq0); + double A = sin(angle * (1.0 - gain)) / sin(angle); + double B = sin(angle * gain) / sin(angle); + dq0 = A + B * dq0; + dq1 = B * dq1; + dq2 = B * dq2; + dq3 = B * dq3; + } else + { + // Lerp (Linear interpolation): + dq0 = (1.0 - gain) + gain * dq0; + dq1 = gain * dq1; + dq2 = gain * dq2; + dq3 = gain * dq3; + } + + normalizeQuaternion(dq0, dq1, dq2, dq3); +} + +void quaternionMultiplication(double p0, double p1, double p2, double p3, + double q0, double q1, double q2, double q3, + double& r0, double& r1, double& r2, double& r3) +{ + // r = p q + r0 = p0 * q0 - p1 * q1 - p2 * q2 - p3 * q3; + r1 = p0 * q1 + p1 * q0 + p2 * q3 - p3 * q2; + r2 = p0 * q2 - p1 * q3 + p2 * q0 + p3 * q1; + r3 = p0 * q3 + p1 * q2 - p2 * q1 + p3 * q0; +} + +void rotateVectorByQuaternion(double x, double y, double z, double q0, + double q1, double q2, double q3, double& vx, + double& vy, double& vz) +{ + vx = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * x + + 2 * (q1 * q2 - q0 * q3) * y + 2 * (q1 * q3 + q0 * q2) * z; + vy = 2 * (q1 * q2 + q0 * q3) * x + + (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * y + + 2 * (q2 * q3 - q0 * q1) * z; + vz = 2 * (q1 * q3 - q0 * q2) * x + 2 * (q2 * q3 + q0 * q1) * y + + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * z; +} + +} // namespace imu_tools diff --git a/src/rm_perception/imu_complementary_filter/src/complementary_filter_node.cpp b/src/rm_perception/imu_complementary_filter/src/complementary_filter_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4f902841f2a2ab2c549ee63ded02f5834f3f8f0c --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/src/complementary_filter_node.cpp @@ -0,0 +1,43 @@ +/* + @author Roberto G. Valenti + + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + + 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 the copyright holder 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#include "imu_complementary_filter/complementary_filter_ros.h" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto filter = std::make_shared(); + rclcpp::spin(filter); + rclcpp::shutdown(); + return 0; +} diff --git a/src/rm_perception/imu_complementary_filter/src/complementary_filter_ros.cpp b/src/rm_perception/imu_complementary_filter/src/complementary_filter_ros.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e8a9dd7465f4fe2827596ee66624be2ec25e5778 --- /dev/null +++ b/src/rm_perception/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -0,0 +1,305 @@ +/* + @author Roberto G. Valenti + + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + + 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 the copyright holder 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#include "imu_complementary_filter/complementary_filter_ros.h" + +#include +#include +#include +#include +#include + +namespace imu_tools { + +ComplementaryFilterROS::ComplementaryFilterROS() + : Node("ComplementaryFilterROS"), + tf_broadcaster_(this), + initialized_filter_(false) +{ + RCLCPP_INFO(this->get_logger(), "Starting ComplementaryFilterROS"); + initializeParams(); + + int queue_size = 5; + + // Register publishers: + // TODO: Check why ros::names::resolve is need here + imu_publisher_ = this->create_publisher("imu/data", queue_size); + + if (publish_debug_topics_) + { + rpy_publisher_ = + this->create_publisher( + "imu/rpy/filtered", queue_size); + + if (filter_.getDoBiasEstimation()) + { + state_publisher_ = this->create_publisher( + "/imu/steady_state", queue_size); + } + } + + // Register IMU raw data subscriber. + imu_subscriber_.reset(new ImuSubscriber(this, "/imu/data_raw")); + + // Register magnetic data subscriber. + if (use_mag_) + { + mag_subscriber_.reset(new MagSubscriber(this, "/imu/mag")); + + sync_.reset(new Synchronizer(SyncPolicy(queue_size), *imu_subscriber_, + *mag_subscriber_)); + sync_->registerCallback(&ComplementaryFilterROS::imuMagCallback, this); + } else + { + imu_subscriber_->registerCallback(&ComplementaryFilterROS::imuCallback, + this); + } +} + +ComplementaryFilterROS::~ComplementaryFilterROS() +{ + RCLCPP_INFO(this->get_logger(), "Destroying ComplementaryFilterROS"); +} + +void ComplementaryFilterROS::initializeParams() +{ + double gain_acc; + double gain_mag; + bool do_bias_estimation; + double bias_alpha; + bool do_adaptive_gain; + double orientation_stddev; + + fixed_frame_ = this->declare_parameter("fixed_frame", "odom"); + use_mag_ = this->declare_parameter("use_mag", false); + publish_tf_ = this->declare_parameter("publish_tf", false); + reverse_tf_ = this->declare_parameter("reverse_tf", false); + constant_dt_ = this->declare_parameter("constant_dt", 0.0); + publish_debug_topics_ = + this->declare_parameter("publish_debug_topics", false); + gain_acc = this->declare_parameter("gain_acc", 0.01); + gain_mag = this->declare_parameter("gain_mag", 0.01); + do_bias_estimation = + this->declare_parameter("do_bias_estimation", true); + bias_alpha = this->declare_parameter("bias_alpha", 0.01); + do_adaptive_gain = this->declare_parameter("do_adaptive_gain", true); + orientation_stddev = + this->declare_parameter("orientation_stddev", 0.0); + orientation_variance_ = orientation_stddev * orientation_stddev; + + filter_.setDoBiasEstimation(do_bias_estimation); + filter_.setDoAdaptiveGain(do_adaptive_gain); + + if (!filter_.setGainAcc(gain_acc)) + RCLCPP_WARN(this->get_logger(), + "Invalid gain_acc passed to ComplementaryFilter."); + if (use_mag_) + { + if (!filter_.setGainMag(gain_mag)) + RCLCPP_WARN(this->get_logger(), + "Invalid gain_mag passed to ComplementaryFilter."); + } + if (do_bias_estimation) + { + if (!filter_.setBiasAlpha(bias_alpha)) + RCLCPP_WARN(this->get_logger(), + "Invalid bias_alpha passed to ComplementaryFilter."); + } + + // check for illegal constant_dt values + if (constant_dt_ < 0.0) + { + // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt + // otherwise, it will be constant + RCLCPP_WARN( + this->get_logger(), + "constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", + constant_dt_); + constant_dt_ = 0.0; + } +} + +void ComplementaryFilterROS::imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw) +{ + const geometry_msgs::msg::Vector3 &a = imu_msg_raw->linear_acceleration; + const geometry_msgs::msg::Vector3 &w = imu_msg_raw->angular_velocity; + const rclcpp::Time &time = imu_msg_raw->header.stamp; + + // Initialize. + if (!initialized_filter_) + { + time_prev_ = time; + initialized_filter_ = true; + return; + } + + // determine dt: either constant, or from IMU timestamp + double dt; + if (constant_dt_ > 0.0) + dt = constant_dt_; + else + dt = (time - time_prev_).nanoseconds() * 1e-9; + + time_prev_ = time; + + // Update the filter. + filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt); + + // Publish state. + publish(imu_msg_raw); +} + +void ComplementaryFilterROS::imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, + MagMsg::ConstSharedPtr mag_msg) +{ + const geometry_msgs::msg::Vector3 &a = imu_msg_raw->linear_acceleration; + const geometry_msgs::msg::Vector3 &w = imu_msg_raw->angular_velocity; + const geometry_msgs::msg::Vector3 &m = mag_msg->magnetic_field; + const rclcpp::Time &time = imu_msg_raw->header.stamp; + + // Initialize. + if (!initialized_filter_) + { + time_prev_ = time; + initialized_filter_ = true; + return; + } + + // Calculate dt. + double dt = (time - time_prev_).nanoseconds() * 1e-9; + time_prev_ = time; + // ros::Time t_in, t_out; + // t_in = ros::Time::now(); + // Update the filter. + if (std::isnan(m.x) || std::isnan(m.y) || std::isnan(m.z)) + filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt); + else + filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, m.x, m.y, m.z, dt); + + // t_out = ros::Time::now(); + // float dt_tot = (t_out - t_in).toSec() * 1000.0; // In msec. + // printf("%.6f\n", dt_tot); + // Publish state. + publish(imu_msg_raw); +} + +tf2::Quaternion ComplementaryFilterROS::hamiltonToTFQuaternion(double q0, + double q1, + double q2, + double q3) const +{ + // ROS uses the Hamilton quaternion convention (q0 is the scalar). However, + // the ROS quaternion is in the form [x, y, z, w], with w as the scalar. + return tf2::Quaternion(q1, q2, q3, q0); +} + +void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) +{ + // Get the orientation: + double q0, q1, q2, q3; + filter_.getOrientation(q0, q1, q2, q3); + tf2::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3); + + // Create and publish fitlered IMU message. + ImuMsg::SharedPtr imu_msg = std::make_shared(*imu_msg_raw); + imu_msg->orientation.x = q1; + imu_msg->orientation.y = q2; + imu_msg->orientation.z = q3; + imu_msg->orientation.w = q0; + + imu_msg->orientation_covariance[0] = orientation_variance_; + imu_msg->orientation_covariance[1] = 0.0; + imu_msg->orientation_covariance[2] = 0.0; + imu_msg->orientation_covariance[3] = 0.0; + imu_msg->orientation_covariance[4] = orientation_variance_; + imu_msg->orientation_covariance[5] = 0.0; + imu_msg->orientation_covariance[6] = 0.0; + imu_msg->orientation_covariance[7] = 0.0; + imu_msg->orientation_covariance[8] = orientation_variance_; + + // Account for biases. + if (filter_.getDoBiasEstimation()) + { + imu_msg->angular_velocity.x -= filter_.getAngularVelocityBiasX(); + imu_msg->angular_velocity.y -= filter_.getAngularVelocityBiasY(); + imu_msg->angular_velocity.z -= filter_.getAngularVelocityBiasZ(); + } + + imu_publisher_->publish(*imu_msg); + + if (publish_debug_topics_) + { + // Create and publish roll, pitch, yaw angles + geometry_msgs::msg::Vector3Stamped rpy; + rpy.header = imu_msg_raw->header; + + tf2::Matrix3x3 M; + M.setRotation(q); + M.getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z); + rpy_publisher_->publish(rpy); + + // Publish whether we are in the steady state, when doing bias + // estimation + if (filter_.getDoBiasEstimation()) + { + std_msgs::msg::Bool state_msg; + state_msg.data = filter_.getSteadyState(); + state_publisher_->publish(state_msg); + } + } + + if (publish_tf_) + { + // Create and publish the ROS tf. + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = imu_msg_raw->header.stamp; + transform.transform.rotation.x = q1; + transform.transform.rotation.y = q2; + transform.transform.rotation.z = q3; + transform.transform.rotation.w = q0; + + if (reverse_tf_) + { + transform.header.frame_id = imu_msg_raw->header.frame_id; + transform.child_frame_id = fixed_frame_; + tf_broadcaster_.sendTransform(transform); + } else + { + transform.child_frame_id = imu_msg_raw->header.frame_id; + transform.header.frame_id = fixed_frame_; + tf_broadcaster_.sendTransform(transform); + } + } +} + +} // namespace imu_tools diff --git a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml index 287e4fd6ca32a8a26c4670336e10c4e2c242afa3..3ccaa27d2fca345229aa7be5c9a254ce0739a255 100644 --- a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml +++ b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml @@ -23,6 +23,6 @@ ground_segmentation: latch: false # latch output topics or not visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING - input_topic: "/mid360_PointCloud2" + input_topic: "/livox/lidar_PointCloud2" obstacle_output_topic: "segmentation/obstacle" - ground_output_topic: "segmentation/ground" + ground_output_topic: "segmentation/ground" \ No newline at end of file diff --git a/src/rm_perception/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py b/src/rm_perception/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py index 79b145ab026608531d8e065009c5d7cb1bf02f1d..02f91197c9db0a68ee0ba2e53df853997d410b91 100644 --- a/src/rm_perception/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py +++ b/src/rm_perception/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): remappings=[('cloud_in', ['/segmentation/obstacle']), ('scan', ['/scan'])], parameters=[{ - # 'target_frame': 'livox_frame', + 'target_frame': 'livox_frame', 'transform_tolerance': 0.01, 'min_height': -1.0, 'max_height': 0.1, @@ -30,4 +30,4 @@ def generate_launch_description(): }], name='pointcloud_to_laserscan' ) - ]) + ]) \ No newline at end of file diff --git a/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp b/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp index 77a78b40682b6087fb8e6cf4e770dcc896b6c6cb..d6babbf24f1d3c7a2f7e2399d62c7e4141aef231 100644 --- a/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp +++ b/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp @@ -123,104 +123,87 @@ namespace gazebo } } - void LivoxPointsPlugin::OnNewLaserScans() - { - // 检查是否已经初始化了 rayShape - if (rayShape) - { - std::vector> points_pair; - // 初始化射线扫描点对 - InitializeRays(points_pair, rayShape); - rayShape->Update(); - - // 创建激光扫描消息并设置时间戳 - msgs::Set(laserMsg.mutable_time(), world->SimTime()); - msgs::LaserScan *scan = laserMsg.mutable_scan(); - InitializeScan(scan); - - // 创建自定义消息 pp_livox，用于发布 Livox CustomMsg 类型消息 - livox_ros_driver2::msg::CustomMsg pp_livox; - pp_livox.header.stamp = node_->get_clock()->now(); - pp_livox.header.frame_id = raySensor->Name(); - int count = 0; - boost::chrono::high_resolution_clock::time_point start_time = boost::chrono::high_resolution_clock::now(); - - // 用于 PointCloud2 类型消息发布 - sensor_msgs::msg::PointCloud cloud; - cloud.header.stamp = node_->get_clock()->now(); - cloud.header.frame_id = raySensor->Name(); - auto &clouds = cloud.points; - - // 遍历射线扫描点对 - for (auto &pair : points_pair) - { - auto range = rayShape->GetRange(pair.first); - auto intensity = rayShape->GetRetro(pair.first); - - // 处理超出范围的数据 - if (range >= RangeMax()) - { - range = 0; - } - else if (range <= RangeMin()) - { - range = 0; - } - - // 计算点云数据 - auto rotate_info = pair.second; - ignition::math::Quaterniond ray; - ray.Euler(ignition::math::Vector3d(0.0, rotate_info.zenith, rotate_info.azimuth)); - auto axis = ray * ignition::math::Vector3d(1.0, 0.0, 0.0); - auto point = range * axis; - - // 填充 CustomMsg 点云消息 - livox_ros_driver2::msg::CustomPoint p; - p.x = point.X(); - p.y = point.Y(); - p.z = point.Z(); - p.reflectivity = intensity; - - // 填充 PointCloud 点云消息 - clouds.emplace_back(); - clouds.back().x = point.X(); - clouds.back().y = point.Y(); - clouds.back().z = point.Z(); - - - // 填充 PointCloud 点云消息 - clouds.emplace_back(); - clouds.back().x = point.X(); - clouds.back().y = point.Y(); - clouds.back().z = point.Z(); - - - // 计算时间戳偏移 - boost::chrono::high_resolution_clock::time_point end_time = boost::chrono::high_resolution_clock::now(); - boost::chrono::nanoseconds elapsed_time = boost::chrono::duration_cast(end_time - start_time); - p.offset_time = elapsed_time.count(); - - // 将点云数据添加到 CustomMsg 消息中 - pp_livox.points.push_back(p); - count ++; - } - - if (scanPub && scanPub->HasConnections()) scanPub->Publish(laserMsg); + void LivoxPointsPlugin::OnNewLaserScans() { + if (!rayShape) { + return; // 检查是否已经初始化了 rayShape + } - // 设置点云数据数量并发布 CustomMsg 消息 - pp_livox.point_num = count; - custom_pub->publish(pp_livox); + std::vector> points_pair; + InitializeRays(points_pair, rayShape); + rayShape->Update(); + + msgs::Set(laserMsg.mutable_time(), world->SimTime()); + msgs::LaserScan *scan = laserMsg.mutable_scan(); + InitializeScan(scan); + + // 创建自定义消息 pp_livox，用于发布 Livox CustomMsg 类型消息 + livox_ros_driver2::msg::CustomMsg pp_livox; + pp_livox.header.stamp = node_->get_clock()->now(); + pp_livox.header.frame_id = raySensor->Name(); + int count = 0; + boost::chrono::high_resolution_clock::time_point start_time = boost::chrono::high_resolution_clock::now(); + + // 用于 PointCloud2 类型消息发布 + sensor_msgs::msg::PointCloud cloud; + cloud.header.stamp = node_->get_clock()->now(); + cloud.header.frame_id = raySensor->Name(); + auto &clouds = cloud.points; + + // 遍历射线扫描点对 + for (const auto &pair : points_pair) { + auto range = rayShape->GetRange(pair.first); + auto intensity = rayShape->GetRetro(pair.first); + + // 处理超出范围的数据 + if (range <= RangeMin() || range >= RangeMax()) { + range = 0; + } - // 发布 Pointcloud2 类型消息 - sensor_msgs::msg::PointCloud2 cloud2; - sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2); - cloud2.header = cloud.header; - cloud2_pub->publish(cloud2); + // 计算点云数据 + auto rotate_info = pair.second; + ignition::math::Quaterniond ray; + ray.Euler(ignition::math::Vector3d(0.0, rotate_info.zenith, rotate_info.azimuth)); + auto axis = ray * ignition::math::Vector3d(1.0, 0.0, 0.0); + auto point = range * axis; + + // 填充 CustomMsg 点云消息 + livox_ros_driver2::msg::CustomPoint p; + p.x = point.X(); + p.y = point.Y(); + p.z = point.Z(); + p.reflectivity = intensity; + + // 填充 PointCloud 点云消息 + clouds.emplace_back(); + clouds.back().x = point.X(); + clouds.back().y = point.Y(); + clouds.back().z = point.Z(); + + // 计算时间戳偏移 + boost::chrono::high_resolution_clock::time_point end_time = boost::chrono::high_resolution_clock::now(); + boost::chrono::nanoseconds elapsed_time = boost::chrono::duration_cast(end_time - start_time); + p.offset_time = elapsed_time.count(); + + // 将点云数据添加到 CustomMsg 消息中 + pp_livox.points.push_back(p); + count++; + } + if (scanPub && scanPub->HasConnections()) { + scanPub->Publish(laserMsg); } + + // 发布 CustomMsg 消息 + pp_livox.point_num = count; + custom_pub->publish(pp_livox); + + // 发布 Pointcloud2 类型消息 + sensor_msgs::msg::PointCloud2 cloud2; + sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2); + cloud2.header = cloud.header; + cloud2_pub->publish(cloud2); } - void LivoxPointsPlugin::InitializeRays(std::vector> &points_pair, boost::shared_ptr &ray_shape) { @@ -403,4 +386,4 @@ namespace gazebo } -} +} \ No newline at end of file diff --git a/src/rm_simulation/livox_laser_simulation_RO2/urdf/mid360.xacro b/src/rm_simulation/livox_laser_simulation_RO2/urdf/mid360.xacro index 60f681eae6800ca7672a37baadde6b929fe09c82..7f068636abebd82b10cd848437c790f5a9a97da8 100644 --- a/src/rm_simulation/livox_laser_simulation_RO2/urdf/mid360.xacro +++ b/src/rm_simulation/livox_laser_simulation_RO2/urdf/mid360.xacro @@ -27,11 +27,12 @@ - - - - - + + + + + + diff --git a/src/rm_simulation/pb_rm_simulation/meshes/mid360.stl b/src/rm_simulation/pb_rm_simulation/meshes/mid360.stl new file mode 100644 index 0000000000000000000000000000000000000000..5c7fdb786e061c00994700924ca2af4b26ed3824 Binary files /dev/null and b/src/rm_simulation/pb_rm_simulation/meshes/mid360.stl differ diff --git a/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro b/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro index 3a7f4eb66ea627edcc85dc93f4917c760508208a..5696af5b645cbe3b0ad31143c21fc49fded161b9 100644 --- a/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro +++ b/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro @@ -13,26 +13,6 @@ - - - - - - - - - - - Gazebo/Green - false - - - - - - - - @@ -61,14 +41,6 @@ - @@ -87,14 +59,6 @@ - @@ -113,14 +77,6 @@ - @@ -139,14 +95,6 @@ - @@ -246,23 +194,24 @@ - true - 20.0 - / - cmd_vel - odom - odom + + + cmd_vel:=cmd_vel + odom:=odom + + + 100 + 100 + + false + false + base_to_wheel1 base_to_wheel3 base_to_wheel2 base_to_wheel4 0.26 0.12 - 20.0 - 2 - base_footprint - true - true @@ -276,7 +225,7 @@ true __default_topic__ - imu + /livox/imu imu_link 100.0 0.0 @@ -289,7 +238,7 @@ 0.0 0.0 - ~/out:=/imu + ~/out:=/livox/imu @@ -297,7 +246,7 @@ - +