[fix] fix some bugs

navigation_msgs/CHANGELOG.rst

@@ -0,0 +1,7 @@
+Changelog
+=========
+
+Version 1.5 (2023-12-25)
+-------------
+* 新增开关门接口
+* 合并楼梯点位信息，优化采点流程

navigation_msgs/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.5)
+project(daystar_navigation_msgs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+# expended interfaces
+rosidl_generate_interfaces(${PROJECT_NAME}
+  msg/CloudInfo.msg
+  msg/Vector2i.msg
+  msg/Trajectory2i.msg
+  msg/NavMapInfo.msg
+  msg/Guardian.msg
+  srv/NavigateToPose.srv
+  srv/SaveMap.srv
+  srv/LoadMap.srv
+  srv/GenerateMaps.srv
+  srv/GenerateStairsPoints.srv
+  srv/StairsPointsMapInfo.srv
+  srv/Stairs.srv
+  srv/Guardian.srv
+  action/FollowWaypoints.action
+  srv/DoorInfo.srv
+  srv/GetStairsPoints.srv
+  DEPENDENCIES std_msgs sensor_msgs geometry_msgs nav_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

navigation_msgs/README.md

@@ -0,0 +1,278 @@
+# navigation_msgs
+## interfaces
+* **daystar_navigation_msgs/msg/CloudInfo**
+```
+# Cloud Info
+std_msgs/Header header
+        builtin_interfaces/Time stamp
+                int32 sec
+                uint32 nanosec
+        string frame_id
+
+int32[] start_ring_index #start index in fullcloud points vector of each ring
+int32[] end_ring_index #end index in fullcloud points vector of each ring
+
+int32[]  point_col_ind # point column index in range image
+float32[] point_range # point range
+
+int64 imu_available #if true, attitude(roll, pitch, yaw) of lidar frame from imu is available
+int64 odom_available #if true,initial pose of lidar frame from imu preintegrated odometry is available
+
+# Attitude for LOAM initialization
+float32 imu_roll_init
+float32 imu_pitch_init
+float32 imu_yaw_init
+
+# Initial guess from imu pre-integration
+float32 initial_guess_x
+float32 initial_guess_y
+float32 initial_guess_z
+float32 initial_guess_roll
+float32 initial_guess_pitch
+float32 initial_guess_yaw
+
+# Point cloud messages
+sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed
+        #
+        std_msgs/Header header
+                builtin_interfaces/Time stamp
+                        int32 sec
+                        uint32 nanosec
+                string frame_id
+        uint32 height
+        uint32 width
+        PointField[] fields
+                uint8 INT8    = 1
+                uint8 UINT8   = 2
+                uint8 INT16   = 3
+                uint8 UINT16  = 4
+                uint8 INT32   = 5
+                uint8 UINT32  = 6
+                uint8 FLOAT32 = 7
+                uint8 FLOAT64 = 8
+                string name
+                uint32 offset
+                uint8  datatype
+                uint32 count
+        bool    is_bigendian
+        uint32  point_step
+        uint32  row_step
+        uint8[] data
+        bool is_dense
+sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature
+        #
+        std_msgs/Header header
+                builtin_interfaces/Time stamp
+                        int32 sec
+                        uint32 nanosec
+                string frame_id
+        uint32 height
+        uint32 width
+        PointField[] fields
+                uint8 INT8    = 1
+                uint8 UINT8   = 2
+                uint8 INT16   = 3
+                uint8 UINT16  = 4
+                uint8 INT32   = 5
+                uint8 UINT32  = 6
+                uint8 FLOAT32 = 7
+                uint8 FLOAT64 = 8
+                string name
+                uint32 offset
+                uint8  datatype
+                uint32 count
+        bool    is_bigendian
+        uint32  point_step
+        uint32  row_step
+        uint8[] data
+        bool is_dense
+sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature
+        #
+        std_msgs/Header header
+                builtin_interfaces/Time stamp
+                        int32 sec
+                        uint32 nanosec
+                string frame_id
+        uint32 height
+        uint32 width
+        PointField[] fields
+                uint8 INT8    = 1
+                uint8 UINT8   = 2
+                uint8 INT16   = 3
+                uint8 UINT16  = 4
+                uint8 INT32   = 5
+                uint8 UINT32  = 6
+                uint8 FLOAT32 = 7
+                uint8 FLOAT64 = 8
+                string name
+                uint32 offset
+                uint8  datatype
+                uint32 count
+        bool    is_bigendian
+        uint32  point_step
+        uint32  row_step
+        uint8[] data
+        bool is_dense
+```
+* **daystar_navigation_msgs/msg/NavMapInfo**
+```
+string floor
+
+float32 lower_height
+float32 upper_height
+
+float32 resolution 0.05
+```
+* **daystar_navigation_msgs/msg/Vector2i**
+```
+uint32 x
+uint32 y
+float32 yaw
+```
+* **daystar_navigation_msgs/msg/Trajectory2i**
+```
+daystar_navigation_msgs/Vector2i[] points
+        uint32 x
+        uint32 y
+        float32 yaw
+```
+* **daystar_navigation_msgs/srv/NavigateToPose**
+```
+daystar_navigation_msgs/srv/NavigateToPose
+string goal_id
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/GetStairsPoints**
+```
+string upstairs
+string point_name
+bool change_map
+string floor_name
+string map_name
+bool door
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/LoadMap**
+```
+# maps file structure
+#map_dir/
+#   ├── L1.png
+#   ├── L1.yaml
+#   ├── L2.png
+#   ├── L2.yaml
+#   └── map.pcd
+
+string map_dir
+string floor
+geometry_msgs/Transform init_pose
+        Vector3 translation
+                float64 x
+                float64 y
+                float64 z
+        Quaternion rotation
+                float64 x 0
+                float64 y 0
+                float64 z 0
+                float64 w 1
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/GenerateStairsPoints**
+```
+string upstairs
+string point_name
+bool change_map
+bool door
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/DoorInfo**
+```
+string door_status
+string door_id
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/Stairs**
+```
+string upstairs
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/StairsPointsMapInfo**
+```
+string floor_name
+string map_name
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/SaveMap**
+```
+string file_destination
+#NOTE: has to be float32 or float64
+float64 resolution
+---
+bool result
+```
+* **daystar_navigation_msgs/srv/GenerateMaps**
+```
+string map_file
+string output_dir
+string map_frame_id map
+float32 downsample_size 0.05
+
+bool is_rep105 true
+# transform map's cordinate to REP105 specifies ref: https://www.ros.org/reps/rep-0105.html
+geometry_msgs/Transform align_transform
+        Vector3 translation
+                float64 x
+                float64 y
+                float64 z
+        Quaternion rotation
+                float64 x 0
+                float64 y 0
+                float64 z 0
+                float64 w 1
+daystar_navigation_msgs/NavMapInfo[] nav_maps
+        string floor
+        float32 lower_height
+        float32 upper_height
+        float32 resolution 0.05
+---
+bool result
+
+```
+* **daystar_navigation_msgs/action/FollowWaypoints**
+```
+#goal definition
+nav_msgs/Path path
+        std_msgs/Header header
+                builtin_interfaces/Time stamp
+                        int32 sec
+                        uint32 nanosec
+                string frame_id
+        geometry_msgs/PoseStamped[] poses
+                std_msgs/Header header
+                        builtin_interfaces/Time stamp
+                                int32 sec
+                                uint32 nanosec
+                        string frame_id
+                Pose pose
+                        Point position
+                                float64 x
+                                float64 y
+                                float64 z
+                        Quaternion orientation
+                                float64 x 0
+                                float64 y 0
+                                float64 z 0
+                                float64 w 1
+---
+#result
+bool result
+---
+#feedback
+float32 distance_to_goal
+```

navigation_msgs/action/FollowWaypoints.action

@@ -0,0 +1,8 @@
+#goal definition
+nav_msgs/Path path
+---
+#result
+bool result
+---
+#feedback
+float32 distance_to_goal

navigation_msgs/msg/CloudInfo.msg

@@ -0,0 +1,29 @@
+# Cloud Info
+std_msgs/Header header 
+
+int32[] start_ring_index #start index in fullcloud points vector of each ring
+int32[] end_ring_index #end index in fullcloud points vector of each ring
+
+int32[]  point_col_ind # point column index in range image
+float32[] point_range # point range 
+
+int64 imu_available #if true, attitude(roll, pitch, yaw) of lidar frame from imu is available
+int64 odom_available #if true,initial pose of lidar frame from imu preintegrated odometry is available
+
+# Attitude for LOAM initialization
+float32 imu_roll_init
+float32 imu_pitch_init
+float32 imu_yaw_init
+
+# Initial guess from imu pre-integration
+float32 initial_guess_x
+float32 initial_guess_y
+float32 initial_guess_z
+float32 initial_guess_roll
+float32 initial_guess_pitch
+float32 initial_guess_yaw
+
+# Point cloud messages
+sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed
+sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature
+sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature

navigation_msgs/msg/Guardian.msg

@@ -0,0 +1,2 @@
+string trigger_source
+float64 velocity_decay_ratio

navigation_msgs/msg/NavMapInfo.msg

@@ -0,0 +1,6 @@
+string floor
+
+float32 lower_height
+float32 upper_height
+
+float32 resolution 0.05

navigation_msgs/msg/Trajectory2i.msg

@@ -0,0 +1 @@
+daystar_navigation_msgs/Vector2i[] points

navigation_msgs/msg/Vector2i.msg

@@ -0,0 +1,3 @@
+uint32 x
+uint32 y
+float32 yaw

navigation_msgs/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>daystar_navigation_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="zhanghy64@lenovo.com">hyan</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>rosidl_default_generators</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation_msgs/srv/DoorInfo.srv

@@ -0,0 +1,4 @@
+string door_status
+string door_id
+---
+bool result 

navigation_msgs/srv/GenerateMaps.srv

@@ -0,0 +1,12 @@
+string map_file
+string output_dir
+string map_frame_id map
+float32 downsample_size 0.05
+
+bool is_rep105 true
+# transform map's cordinate to REP105 specifies ref: https://www.ros.org/reps/rep-0105.html
+geometry_msgs/Transform align_transform
+
+daystar_navigation_msgs/NavMapInfo[] nav_maps
+---
+bool result

navigation_msgs/srv/GenerateStairsPoints.srv

@@ -0,0 +1,6 @@
+string upstairs
+string point_name
+bool change_map
+bool door
+---
+bool result

navigation_msgs/srv/GetStairsPoints.srv

@@ -0,0 +1,8 @@
+string upstairs
+string point_name
+bool change_map
+string floor_name 
+string map_name
+bool door
+---
+bool result

navigation_msgs/srv/Guardian.srv

@@ -0,0 +1,4 @@
+string trigger_source
+float64 velocity_decay_ratio
+---
+bool result

navigation_msgs/srv/LoadMap.srv

@@ -0,0 +1,13 @@
+# maps file structure
+#map_dir/
+#   ├── L1.png
+#   ├── L1.yaml
+#   ├── L2.png
+#   ├── L2.yaml
+#   └── map.pcd
+
+string map_dir
+string floor
+geometry_msgs/Transform init_pose
+---
+bool result

navigation_msgs/srv/NavigateToPose.srv

@@ -0,0 +1,3 @@
+string goal_id
+---
+bool result

navigation_msgs/srv/SaveMap.srv

@@ -0,0 +1,5 @@
+string file_destination
+#NOTE: has to be float32 or float64
+float64 resolution
+---
+bool result

navigation_msgs/srv/Stairs.srv

@@ -0,0 +1,3 @@
+string upstairs
+---
+bool result

navigation_msgs/srv/StairsPointsMapInfo.srv

@@ -0,0 +1,4 @@
+string floor_name 
+string map_name
+---
+bool result

saturn_controller/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(ysc_robot_msgs REQUIRED)
 find_package(daystar_navigation_msgs REQUIRED)
 find_package(saturn_msgs REQUIRED)
+find_package(spdlog REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
@@ -43,7 +44,7 @@ link_directories(${CMAKE_CURRENT_BINARY_DIR}/../../install/saturn_controller/lib
 file(GLOB main_files src/*.cpp)
 
 add_library(saturn_controller_component SHARED src/saturn_controller.cpp ${main_files})
-target_link_libraries(saturn_controller_component -lmc_client)
+target_link_libraries(saturn_controller_component spdlog::spdlog -lmc_client)
 ament_target_dependencies(saturn_controller_component ${THIS_PACKAGE_INCLUDE_DEPENDS})
 rclcpp_components_register_nodes(saturn_controller_component "saturn_ros2::SaturnController")
 

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -164,4 +164,4 @@ class SaturnController {
 
     void publishRobotMode();
 };
-}  // namespace saturn_ros2
+}  // namespace saturn_ros2

saturn_controller/launch/saturn_ros2.launch.py

@@ -23,6 +23,7 @@ def generate_launch_description():
             launch_ros.descriptions.ComposableNode(
                 package='saturn_controller',
                 plugin='saturn_ros2::SaturnController',
+                namespace = "GS_003",
                 name='saturn_controller',
                 parameters=[config]
             ),

saturn_controller/src/config_handler.cpp

@@ -10,10 +10,8 @@ void ConfigHandler::loadConfig() {
     // 声明参数                                                                
     motion_computer_ip_address_ = node_->declare_parameter("motion_computer_ip_address", "192.168.100.103");
                                                                                
-    main_timer_frequency_ = node_->declare_parameter("main_timer_frequency", 50.0);
-                                                                               
-    imu_timer_frequency_ = node_->declare_parameter("imu_timer_frequency", 300.0);
-                                                                               
+    main_timer_frequency_ = node_->declare_parameter("main_timer_frequency", 50.0);                                                      
+    imu_timer_frequency_ = node_->declare_parameter("imu_timer_frequency", 300.0);                                                     
     legged_odom_timer_frequency_ = node_->declare_parameter("legged_odom_timer_frequency", 100.0);
 }
 }

saturn_controller/src/saturn_controller.cpp

@@ -401,4 +401,4 @@ void SaturnController::publishRobotMode() {
 }
 
 }  // namespace saturn_ros2
-RCLCPP_COMPONENTS_REGISTER_NODE(saturn_ros2::SaturnController)
+RCLCPP_COMPONENTS_REGISTER_NODE(saturn_ros2::SaturnController)

ysc_robot_msgs/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(ysc_robot_msgs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# expended interfaces
+rosidl_generate_interfaces(${PROJECT_NAME}
+
+  # NOTE absolute interfaces file has to be seprated to  absolute base and file name by colon(:)
+  ${PROJECT_SOURCE_DIR}/srv:RobotCommand.srv
+  # or just relative path: srv/RobotCommand.srv
+
+  # DEPENDENCIES std_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

ysc_robot_msgs/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ysc_robot_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>rosidl_default_generators</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ysc_robot_msgs/srv/RobotCommand.srv

@@ -0,0 +1,3 @@
+string comamnd_name
+---
+bool result