Update trajectory_visualizer node

TheNoobInventor · Apr 15, 2024 · 3c3f383 · 3c3f383
1 parent ed13f74
commit 3c3f383
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 264 deletions.
diff --git a/lidarbot_description/meshes/left_wheel.dae b/lidarbot_description/meshes/left_wheel.dae
diff --git a/lidarbot_description/urdf/lidarbot_core.xacro b/lidarbot_description/urdf/lidarbot_core.xacro
@@ -156,15 +156,15 @@
     </xacro:box_inertia>
   </link>
 
-  <!-- Left Wheel -->
+  <!-- Left wheel -->
   <link name="left_wheel">
     <visual>
         <geometry>
           <mesh filename="package://lidarbot_description/meshes/right_wheel.dae" />
         </geometry>
     </visual>
     <collision>
-      <origin xyz="0 0 0.0155" rpy="0 0 0"/>
+      <origin xyz="0 0 -0.0065" rpy="0 0 0"/>
       <geometry>
         <sphere radius="${wheel_radius}"/>
       </geometry>
@@ -179,10 +179,10 @@
     <parent link="base_link"/>
     <child link="left_wheel"/>
     <origin xyz="0.133 0.065 -${wheel_zoff}" rpy="-${pi/2} 0 ${pi}"/>
-    <axis xyz="0 0 1"/>
+    <axis xyz="0 0 -1"/>
   </joint>
 
-  <!-- Right Wheel -->
+  <!-- Right wheel -->
   <link name="right_wheel">
     <visual>
       <geometry>
@@ -238,4 +238,14 @@
     <mu2 value="0.001"/>
   </gazebo>
 
+  <gazebo reference="left_wheel">
+    <mu1 value="1.16"/>
+    <mu2 value="1.16"/>
+  </gazebo>
+
+  <gazebo reference="right_wheel">
+    <mu1 value="1.16"/>
+    <mu2 value="1.16"/>
+  </gazebo>
+
 </robot>
diff --git a/lidarbot_navigation/scripts/trajectory_visualizer (copy).py b/lidarbot_navigation/scripts/trajectory_visualizer (copy).py
diff --git a/lidarbot_navigation/scripts/trajectory_visualizer.py b/lidarbot_navigation/scripts/trajectory_visualizer.py
@@ -20,10 +20,10 @@ def __init__(self, name):
 
         # Declare parameters
         self.max_poses_param = self.declare_parameter("max_poses", 1000).value
-        # self.threshold_param = self.declare_parameter("threshold", 0.001).value
-        self.threshold_param = self.declare_parameter("threshold", 0.005).value
+        self.threshold_param = self.declare_parameter("threshold", 0.001).value
 
-        self.frame_id_param = self.declare_parameter("frame_id", "map").value
+        # self.frame_id_param = self.declare_parameter("frame_id", "map").value
+        self.frame_id_param = self.declare_parameter("frame_id", "odom").value
 
         self.trajectory_path_msg = Path()
         self.previous_pose_position = Point()
@@ -41,24 +41,17 @@ def __init__(self, name):
 
     # Callback function for odometry type messages
     def odom_callback(self, odom_msg):
-        # self.get_logger().info(
-        #     "Received odom message with x: %f and y: %f"
-        #     % (
-        #         odom_msg.pose.pose.position.x,
-        #         odom_msg.pose.pose.position.y,
-        #     )
-        # )
 
         # Process message position and add it to path
         self.publish_trajectory_path(odom_msg.pose.pose.position)
 
     # Add pose and publish trajectory path message
     def publish_trajectory_path(self, position):
+
         # If the pose has moved more than a set threshold, add it ot the path message and publish
         if (abs(self.previous_pose_position.x - position.x) > self.threshold_param) or (
             abs(self.previous_pose_position.y - position.y) > self.threshold_param
         ):
-            # self.get_logger().info("Exceding threshold, adding pose to path")
 
             # Add current pose to path
             self.trajectory_path_msg.header.stamp = self.get_clock().now().to_msg()