final changes

docker_humble_jetson/Dockerfile

@@ -19,6 +19,7 @@ RUN apt-get install -y --no-install-recommends \
     ros-${ROS_DISTRO}-librealsense2*\
     ros-${ROS_DISTRO}-realsense2-*\
     ros-${ROS_DISTRO}-foxglove-bridge \
+    ros-${ROS_DISTRO}-tf-transformations \
     python3-zmq \
     libyaml-cpp-dev \
     lcov \

navigation/path_planning/config/map_server_params.yaml

@@ -1,11 +1,19 @@
 # Generate and publish the static map occupancy grid from the static_costmap image.
 # The pixels in the image have values between 0 and 255 with 0 being an obstacle and 255 being free space.
 # Then the values are scaled to values between 0 and 100 to create the occupancy grid, where 0 is free space 
-# and 100 is occupied space.
+# # and 100 is occupied space.
 image: static_costmap.png
 resolution: 0.195
 origin: [-9.5, -17.5, 0.0]
-occupied_thresh: 1.0
-free_thresh: 1.0
+occupied_thresh: 0.99
+free_thresh: 0
 negate: 0
-mode: scale
+mode: scale
+# and 100 is occupied space.
+# image: static_costmap.png
+# resolution: 0.195
+# origin: [-9.5, -17.5, 0.0]
+# occupied_thresh: 1.0
+# free_thresh: 1.0
+# negate: 0
+# mode: scale

navigation/path_planning/config/nav2_params_real.yaml

@@ -52,7 +52,7 @@ global_costmap:
         plugin: "nav2_costmap_2d::InflationLayer"
         enabled: True
         cost_scaling_factor: 0.0
-        inflation_radius: 0.20
+        inflation_radius: 0.2
         inflate_unknown: False
         inflate_around_unknown: False
       obstacle_layer:
@@ -64,7 +64,7 @@ global_costmap:
           sensor_frame: lidar_link
           data_type: "PointCloud2"
           max_obstacle_height: 4.0
-          min_obstacle_height: -2.0
+          min_obstacle_height: 0.5
           obstacle_max_range: 60.0
           obstacle_min_range: 1.0
           clearing: True
@@ -108,7 +108,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 0.8
-        inflation_radius:  0.6
+        inflation_radius:  0.3
 
       always_send_full_costmap: True
   local_costmap_client:

navigation/path_planning/config/offset.yaml

@@ -0,0 +1,10 @@
+position_offset:
+  - 17.228  # was 11 
+  - -11.549 # was 17
+  - 0.0  
+
+orientation_offset:
+  - 0.0  
+  - 0.0  
+  - 0.7071
+  - 0.7071

navigation/path_planning/launch/nav2_real.launch.py

@@ -81,11 +81,11 @@ def launch_setup(context: launch.LaunchContext, *args, **kwargs):
     return [
         # Commands
         start_nav2_cmd,
-        # start_wheels_control_cmd,
+        start_wheels_control_cmd,
         # Nodes
         # local_robot_localization_node,
-        global_robot_localization_node,
-        odom_offset_node
+        # global_robot_localization_node,
+        # odom_offset_node
     ]
 
 

navigation/path_planning/path_planning/odom_offset_node.py

@@ -4,66 +4,57 @@
 from rclpy.qos import QoSProfile, ReliabilityPolicy
 from nav_msgs.msg import Odometry
 import numpy as np
-# import tf_transformations
+import tf_transformations
 from geometry_msgs.msg import Quaternion
-
+import yaml
+from ament_index_python.packages import get_package_share_directory
+import os
 
 class OdometryOffsetNode(Node):
 
     def __init__(self):
         super().__init__("odom_offset_node")
-
-        # self.declare_parameter('use_config_file', False)
-        # self.use_config_file = self.get_parameter('use_config_file').get_parameter_value().bool_value
-
-        # if self.use_config_file:
-        #     self.declare_parameter('config_file_path', 'config.yaml')
-        #     config_file_path = self.get_parameter('config_file_path').get_parameter_value().string_value
-        #     self.offset_position, self.offset_orientation = self.load_offsets_from_config(config_file_path)
-        # else:
-        #     self.declare_parameter('position_offset', [0.0, 0.0, 0.0])
-        #     self.declare_parameter('orientation_offset', [0.0, 0.0, 0.0, 1.0])
-        #     self.offset_position = np.array(self.get_parameter('position_offset').get_parameter_value().double_array_value)
-        #     self.offset_orientation = self.get_parameter('orientation_offset').get_parameter_value().double_array_value
+
+        package_name = 'path_planning' 
+        config_file_path = os.path.join(get_package_share_directory(package_name), 'config', 'offset.yaml')
+        self.offset_position, self.offset_orientation = self.load_offsets_from_config(config_file_path)
 
         self.odom_subscriber = self.create_subscription(
             Odometry, "/lio_sam/mapping/odometry", self.odom_callback, QoSProfile(depth=10, reliability = ReliabilityPolicy.BEST_EFFORT)
         )
         self.odom_publisher = self.create_publisher(Odometry, "/odom_with_offset", 10)
 
     def load_offsets_from_config(self, config_file_path):
-        import yaml
-
+
         with open(config_file_path, "r") as file:
             config = yaml.safe_load(file)
         position_offset = np.array(config["position_offset"])
         orientation_offset = config["orientation_offset"]
         return position_offset, orientation_offset
 
     def odom_callback(self, msg):
-        # msg.header.frame_id = "odom"
-        # msg.pose.pose.position.x += 0.0
-        # msg.pose.pose.position.y += 
-        # msg.pose.pose.position.z += self.offset_position[2]
+        msg.pose.pose.position.x += self.offset_position[0]
+        msg.pose.pose.position.y += self.offset_position[1]
+        msg.pose.pose.position.z += self.offset_position[2]
 
         current_orientation = [
             msg.pose.pose.orientation.x,
             msg.pose.pose.orientation.y,
             msg.pose.pose.orientation.z,
             msg.pose.pose.orientation.w,
         ]
-        # offset_quaternion = self.offset_orientation
+        offset_quaternion = self.offset_orientation
 
-        # new_orientation = tf_transformations.quaternion_multiply(
-        #     current_orientation, offset_quaternion
-        # )
+        new_orientation = tf_transformations.quaternion_multiply(
+            current_orientation, offset_quaternion
+        )
 
-        # msg.pose.pose.orientation = Quaternion(
-        #     x=new_orientation[0],
-        #     y=new_orientation[1],
-        #     z=new_orientation[2],
-        #     w=new_orientation[3],
-        # )
+        msg.pose.pose.orientation = Quaternion(
+            x=new_orientation[0],
+            y=new_orientation[1],
+            z=new_orientation[2],
+            w=new_orientation[3],
+        )
 
         self.odom_publisher.publish(msg)
 

navigation/path_planning/path_planning/offset.yaml.save

@@ -0,0 +1,10 @@
+position_offset:
+  - 0.1  
+  - 0.2  
+  - 0.0  
+
+orientation_offset:
+  - 0.0  
+  - 0.0  
+  - 0.707106  
+  - 0.7071068 

obstacle_detection/lidar/lidar_filter/launch/filter.launch.py

@@ -6,7 +6,7 @@
 def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument('sim', default_value = 'false'),
-        DeclareLaunchArgument('slope_threshold', default_value = '0.4', description = '1 => 45 °'),
+        DeclareLaunchArgument('slope_threshold', default_value = '1.0', description = '1 => 45 °'),
         DeclareLaunchArgument('min_distance', default_value = '0.8'),
         DeclareLaunchArgument('n_clusters', default_value = '50'),
 

obstacle_detection/lidar/lidar_filter/src/lidar_filter.cpp

@@ -24,6 +24,17 @@ rmw_qos_profile_t real_profile = {
     RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
     false
 };
+rmw_qos_profile_t pub_profile = {
+    RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+    5,
+    RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+    RMW_QOS_POLICY_DURABILITY_VOLATILE,
+    RMW_QOS_DEADLINE_DEFAULT,
+    RMW_QOS_LIFESPAN_DEFAULT,
+    RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
+    RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+    false
+};
 rmw_qos_profile_t sim_profile = {
     RMW_QOS_POLICY_HISTORY_KEEP_LAST,
     5,
@@ -65,7 +76,7 @@ class LidarFilterNode : public rclcpp::Node
         RCLCPP_INFO(this->get_logger(), "Lidar Filter Node Started");
 
         this->declare_parameter("min_distance", 0.8);
-        this->declare_parameter("slope_threshold", 0.4);    //100% is 45 degrees to horizontal
+        this->declare_parameter("slope_threshold", 1.0);    //100% is 45 degrees to horizontal
         this->declare_parameter("filter", true);
         this->declare_parameter("voxel_leaf_size", 0.05);
         this->declare_parameter("n_clusters", 50);
@@ -83,12 +94,19 @@ class LidarFilterNode : public rclcpp::Node
             ),
             profile
         );
+        auto qos_pub = rclcpp::QoS(
+            rclcpp::QoSInitialization(
+                pub_profile.history,
+                pub_profile.depth
+            ),
+            pub_profile
+        );
 
-        std::string topic_name = sim ? "/ouster/points" : "/points";
+        std::string topic_name = "/points";
 
         // Create a publisher
-        pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/lidar_filter/filtered_points", qos);
-        pub_floor = this->create_publisher<sensor_msgs::msg::PointCloud2>("/lidar_filter/floor_points", qos);
+        pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/lidar_filter/filtered_points", qos_pub);
+        pub_floor = this->create_publisher<sensor_msgs::msg::PointCloud2>("/lidar_filter/floor_points", qos_pub);
 
         // Create subscribers lidar points and NAV_status (to destroy node)
         sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(topic_name, qos, std::bind(&LidarFilterNode::callback, this, std::placeholders::_1));