Fix odom drift (#6)

MarqRazz · Oct 29, 2023 · 27d0624 · 27d0624
1 parent 1d0e9a6
commit 27d0624
Show file tree

Hide file tree

Showing 12 changed files with 117 additions and 72 deletions.
diff --git a/.docker/Dockerfile b/.docker/Dockerfile
@@ -78,14 +78,13 @@ RUN echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main"
 RUN mkdir -p /root/c3pzero_ws/src
 WORKDIR /root/c3pzero_ws/src
 COPY ./ ./c3pzero/
-# RUN vcs import < ./c3pzero/c3pzero.repos # I prefer to manage my repos on the host
+RUN vcs import < ./c3pzero/c3pzero.repos
 WORKDIR /root/c3pzero_ws
 # hadolint ignore=SC1091
 RUN source /opt/ros/humble/setup.bash \
     && apt-get update -y \
     && apt-get upgrade -y \
-    # TODO correct rosdep command from requiring -r command (its still looking for robotiq_description)
-    && rosdep install --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" -y -r \
+    && rosdep install --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" -y \
     && rm -rf /var/lib/apt/lists/*
 
 # Use CycloneDDS as the default but also setup Fast DDS to support NVIDIA Isaac Sim.
@@ -97,16 +96,16 @@ COPY ./.docker/cyclonedds_localhost.xml /root/.ros/cyclonedds_localhost.xml
 ENV FASTRTPS_DEFAULT_PROFILES_FILE=/root/.ros/fastdds.xml
 
 # Pre-download the gazebo world so it doesn't have to on the first sim launch
-RUN ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Depot
+RUN ign fuel download -v 4 -u https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depot
 
 # Set up the entrypoint for both container start and interactive terminals.
 COPY ./.docker/ros_entrypoint.sh /root/.ros/
-RUN echo "source /root/.ros/ros_entrypoint.sh" >> ~/.bashrc && \
-    echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc && \
-    echo 'export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{name} {time}]: {message}"' >> /root/.bashrc && \
+RUN echo 'export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{name} {time}]: {message}"' >> /root/.bashrc && \
     echo "export RCUTILS_COLORIZED_OUTPUT=1" >> /root/.bashrc && \
+    echo "alias cyclone='export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp'" >> /root/.bashrc && \
     echo "alias fastdds='export RMW_IMPLEMENTATION=rmw_fastrtps_cpp'" >> /root/.bashrc && \
     echo "alias sws='source ./install/setup.bash'" >> /root/.bashrc && \
     echo "alias symlink_build='colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --event-handlers log-'" >> /root/.bashrc && \
-    echo "export PATH=/usr/lib/ccache:$PATH" >> /root/.bashrc
+    echo "export PATH=/usr/lib/ccache:$PATH" >> /root/.bashrc && \
+    echo "source /root/.ros/ros_entrypoint.sh" >> ~/.bashrc
 WORKDIR /root/c3pzero_ws/
diff --git a/c300/c300_bringup/config/c300_gz_controllers.yaml b/c300/c300_bringup/config/c300_gz_controllers.yaml
@@ -12,13 +12,14 @@ diff_drive_base_controller:
   ros__parameters:
     left_wheel_names: ["drivewhl_l_joint"]
     right_wheel_names: ["drivewhl_r_joint"]
+    wheels_per_side: 1
 
-    wheel_separation: 0.539
+    wheel_separation: 0.61 # outside distance between the wheels
     wheel_radius: 0.1715
 
     wheel_separation_multiplier: 1.0
-    left_wheel_radius_multiplier: 0.89 # TODO figure out why this multiplier needs to be offset!
-    right_wheel_radius_multiplier: 0.89
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
 
     publish_rate: 50.0
     odom_frame_id: odom
@@ -27,6 +28,7 @@ diff_drive_base_controller:
     twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
 
     open_loop: false
+    position_feedback: true
     enable_odom_tf: true
 
     cmd_vel_timeout: 0.5
@@ -39,17 +41,17 @@ diff_drive_base_controller:
     linear.x.has_velocity_limits: true
     linear.x.has_acceleration_limits: true
     linear.x.has_jerk_limits: false
-    linear.x.max_velocity: 1.0
-    linear.x.min_velocity: -1.0
-    linear.x.max_acceleration: 1.0
+    linear.x.max_velocity: 2.0
+    linear.x.min_velocity: -2.0
+    linear.x.max_acceleration: 0.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
     angular.z.has_velocity_limits: true
     angular.z.has_acceleration_limits: true
     angular.z.has_jerk_limits: false
-    angular.z.max_velocity: 1.0
-    angular.z.min_velocity: -1.0
+    angular.z.max_velocity: 2.0
+    angular.z.min_velocity: -2.0
     angular.z.max_acceleration: 1.0
     angular.z.min_acceleration: -1.0
     angular.z.max_jerk: 0.0

diff --git a/c300/c300_bringup/config/c300_isaac_controllers.yaml b/c300/c300_bringup/config/c300_isaac_controllers.yaml
@@ -12,13 +12,14 @@ diff_drive_base_controller:
   ros__parameters:
     left_wheel_names: ["drivewhl_l_joint"]
     right_wheel_names: ["drivewhl_r_joint"]
+    wheels_per_side: 1
 
-    wheel_separation: 0.539
+    wheel_separation: 0.61 # outside distance between the wheels
     wheel_radius: 0.1715
 
     wheel_separation_multiplier: 1.0
-    left_wheel_radius_multiplier: 0.965 # TODO figure out why this multiplier needs to be offset!
-    right_wheel_radius_multiplier: 0.965
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
 
     publish_rate: 50.0
     odom_frame_id: odom
@@ -27,6 +28,7 @@ diff_drive_base_controller:
     twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
 
     open_loop: false
+    position_feedback: true
     enable_odom_tf: true
 
     cmd_vel_timeout: 0.5
@@ -39,17 +41,17 @@ diff_drive_base_controller:
     linear.x.has_velocity_limits: true
     linear.x.has_acceleration_limits: true
     linear.x.has_jerk_limits: false
-    linear.x.max_velocity: 1.0
-    linear.x.min_velocity: -1.0
-    linear.x.max_acceleration: 1.0
+    linear.x.max_velocity: 2.0
+    linear.x.min_velocity: -2.0
+    linear.x.max_acceleration: 0.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
     angular.z.has_velocity_limits: true
     angular.z.has_acceleration_limits: true
     angular.z.has_jerk_limits: false
-    angular.z.max_velocity: 1.0
-    angular.z.min_velocity: -1.0
+    angular.z.max_velocity: 2.0
+    angular.z.min_velocity: -2.0
     angular.z.max_acceleration: 1.0
     angular.z.min_acceleration: -1.0
     angular.z.max_jerk: 0.0

diff --git a/c300/c300_bringup/launch/isaac_c300.launch.py b/c300/c300_bringup/launch/isaac_c300.launch.py
@@ -107,6 +107,7 @@ def generate_launch_description():
     prefix = LaunchConfiguration("prefix")
     diff_drive_controller = LaunchConfiguration("diff_drive_controller")
     launch_rviz = LaunchConfiguration("launch_rviz")
+    sim_isaac = LaunchConfiguration("sim_isaac")
     use_sim_time = LaunchConfiguration("use_sim_time")
 
     robot_controllers = PathJoinSubstitution(

diff --git a/c300/c300_bringup/rviz/bringup_config.rviz b/c300/c300_bringup/rviz/bringup_config.rviz
@@ -1,14 +1,14 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /Status1
         - /LaserScan1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 820
+    Tree Height: 659
 Visualization Manager:
   Class: ""
   Displays:
@@ -150,7 +150,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_footprint
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -168,7 +168,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.4849162101745605
+      Distance: 3.4911282062530518
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -183,18 +183,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3647971749305725
+      Pitch: 1.5497963428497314
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.1853978633880615
+      Yaw: 3.13539719581604
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 1043
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000010000000000000156000003bdfc0200000001fb000000100044006900730070006c006100790073010000003b000003bd000000c700ffffff00000624000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001da00000381fc0200000001fb000000100044006900730070006c0061007900730100000069000003810000017800ffffff0000059a0000038100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Width: 1920
-  X: 0
-  Y: 0
+  X: 151
+  Y: 1223
diff --git a/c300/c300_description/urdf/hokuyo_uam05lp.urdf.xacro b/c300/c300_description/urdf/hokuyo_uam05lp.urdf.xacro
@@ -4,7 +4,13 @@
 
     <xacro:include filename="$(find c300_description)/urdf/hokuyo.gazebo.xacro" />
 
-    <xacro:hokuyo_gazebo frame="${prefix}_laser" topic="${topic}" sim_gazebo="false" sim_ignition="true"/>
+    <xacro:hokuyo_gazebo
+      frame="${prefix}_laser"
+      topic="${topic}"
+      min_angle="${min_angle}"
+      max_angle="${max_angle}"
+      sim_gazebo="false"
+      sim_ignition="true"/>
 
     <joint name="${prefix}_laser_mount_joint" type="fixed">
       <xacro:insert_block name="origin" />

diff --git a/c300/c300_driver/config/f710.config.yaml b/c300/c300_driver/config/f710.config.yaml
@@ -6,12 +6,14 @@ teleop_twist_joy_node:
     scale_linear:
       x: 1.0
     scale_linear_turbo:
-      x: 1.5
+      x: 2.0
 
     axis_angular:  # Twist
-      yaw: 2
+      yaw: 3
     scale_angular:
-      yaw: 0.5
+      yaw: 0.8
+    scale_angular_turbo:
+      yaw: 2.0
 
     enable_button: 5  # Trigger
     enable_turbo_button: 4  # Button 2 aka thumb button
diff --git a/c300/c300_navigation/params/nav2_params.yaml b/c300/c300_navigation/params/nav2_params.yaml
@@ -14,7 +14,7 @@ amcl:
     global_frame_id: "map"
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
+    laser_max_range: 30.0
     laser_min_range: -1.0
     laser_model_type: "likelihood_field"
     max_beams: 60
@@ -123,8 +123,8 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.15
+      yaw_goal_tolerance: 0.15
 
     # DWB parameters
     FollowPath:
@@ -134,24 +134,24 @@ controller_server:
       min_vel_y: 0.0
       max_vel_x: 1.0
       max_vel_y: 0.0
-      max_vel_theta: 1.0
+      max_vel_theta: 0.8
       min_speed_xy: 0.0
       max_speed_xy: 1.0
       min_speed_theta: 0.0
-      acc_lim_x: 2.5
+      acc_lim_x: 0.5
       acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -3.5
+      acc_lim_theta: 1.0
+      decel_lim_x: -0.5
       decel_lim_y: 0.0
-      decel_lim_theta: -3.2
+      decel_lim_theta: -1.0
       vx_samples: 20
       vy_samples: 5
       vtheta_samples: 20
       sim_time: 1.7  # Time to simulate ahead by (s)
       linear_granularity: 0.05
       angular_granularity: 0.025
       transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.15
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
@@ -184,8 +184,8 @@ local_costmap:
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        inflation_radius: 0.65 # Adjust inflation_radius to change how close robot comes to obstacles
-        cost_scaling_factor: 3.0
+        inflation_radius: 0.75 # Adjust inflation_radius to change how close robot comes to obstacles
+        cost_scaling_factor: 5.0
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
@@ -255,8 +255,8 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55 # Can change how much to inflate costmap around obstacles
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.75 # Can change how much to inflate costmap around obstacles
       always_send_full_costmap: True
 
 map_server:
@@ -336,11 +336,11 @@ velocity_smoother:
     use_sim_time: True
     smoothing_frequency: 20.0
     scale_velocities: False
-    feedback: "OPEN_LOOP"
-    max_velocity: [1.0, 0.0, 1.0] # [x, y, theta]
-    min_velocity: [-1.0, 0.0, -1.0] # [x, y, theta]
-    max_accel: [0.6, 0.0, 2.0]
-    max_decel: [-0.6, 0.0, -2.0]
+    feedback: "CLOSED_LOOP"
+    max_velocity: [1.0, 0.0, 0.8] # [x, y, theta]
+    min_velocity: [-1.0, 0.0, -0.8] # [x, y, theta]
+    max_accel: [1.0, 0.0, 1.0]
+    max_decel: [-1.0, 0.0, -1.0]
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]

diff --git a/c3pzero.repos b/c3pzero.repos
@@ -5,15 +5,19 @@ repositories:
     version: main
   ros2_kortex:
     type: git
-    url: git@github.com:Kinovarobotics/ros2_kortex.git
+    url: https://github.com/Kinovarobotics/ros2_kortex.git
     version: main
+  ros2_controllers:
+    type: git
+    url: https://github.com/MarqRazz/ros2_controllers.git
+    version: fix_diff_drive_odom
   realsense-ros:
     type: git
-    url: git@github.com:MarqRazz/realsense-ros.git
+    url: https://github.com/MarqRazz/realsense-ros.git
     version: ros2-development
   ros2_robotiq_gripper:
     type: git
-    url: git@github.com:PickNikRobotics/ros2_robotiq_gripper.git
+    url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
     version: main
   serial:
     type: git

diff --git a/c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml b/c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml
@@ -18,8 +18,9 @@ diff_drive_base_controller:
   ros__parameters:
     left_wheel_names: ["drivewhl_l_joint"]
     right_wheel_names: ["drivewhl_r_joint"]
+    wheels_per_side: 1
 
-    wheel_separation: 0.540
+    wheel_separation: 0.61 # outside distance between the wheels
     wheel_radius: 0.1715
 
     wheel_separation_multiplier: 1.0
@@ -32,7 +33,8 @@ diff_drive_base_controller:
     pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
     twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
 
-    open_loop: true
+    open_loop: false
+    position_feedback: true
     enable_odom_tf: true
 
     cmd_vel_timeout: 0.5
@@ -45,17 +47,17 @@ diff_drive_base_controller:
     linear.x.has_velocity_limits: true
     linear.x.has_acceleration_limits: true
     linear.x.has_jerk_limits: false
-    linear.x.max_velocity: 1.0
-    linear.x.min_velocity: -1.0
-    linear.x.max_acceleration: 3.0
+    linear.x.max_velocity: 2.0
+    linear.x.min_velocity: -2.0
+    linear.x.max_acceleration: 0.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
     angular.z.has_velocity_limits: true
     angular.z.has_acceleration_limits: true
     angular.z.has_jerk_limits: false
-    angular.z.max_velocity: 1.0
-    angular.z.min_velocity: -1.0
+    angular.z.max_velocity: 2.0
+    angular.z.min_velocity: -2.0
     angular.z.max_acceleration: 1.0
     angular.z.min_acceleration: -1.0
     angular.z.max_jerk: 0.0