Kinda working again

src/dcaiti_control/config/dcaiti_config.yml

@@ -2,40 +2,34 @@ controller_manager:
   ros__parameters:
     update_rate: 30
 
-    diff_cont:
+    ackermann_steering_controller:
-      type: diff_drive_controller/DiffDriveController
+      type: 'ackermann_steering_controller/AckermannSteeringController'
     
     joint_broad:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-diff_cont:
+ackermann_steering_controller:
   ros__parameters:
 
     publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
     base_frame_id: base_link
     publish_limited_velocity: true
+    in_chained_mode: false
 
-    left_wheel_names: ['left_wheel_joint']
+    reference_timeout: 2.0
-    right_wheel_names: ['right_wheel_joint']
+    front_steering: true
-    wheel_separation: 0.215
+    open_loop: false
-    wheel_radius: 0.049338032
+    velocity_rolling_window_size: 10
-
+    position_feedback: true
     use_stamped_vel: false
+    rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
+    front_wheels_names: [front_right_steer_joint, front_left_steer_joint]
 
-    linear:
+    wheelbase: 3.24644
-      x:
+    front_wheel_track: 2.12321
-        has_velocity_limits    : true
+    rear_wheel_track: 1.76868
-        max_velocity           : 1.0  # m/s
+    front_wheels_radius: 0.45
-        min_velocity           : -0.5 # m/s
+    rear_wheels_radius: 0.45
-        has_acceleration_limits: true
-        max_acceleration       : 0.6  # m/s^2
-        min_acceleration       : -0.3 # m/s^2
-    angular:
-      z:
-        has_velocity_limits    : true
-        max_velocity           : 1.0  # rad/s
-        has_acceleration_limits: true
-        max_acceleration       : 0.8  # rad/s^2
 
 
 joint_limits:

src/dcaiti_control/description/bbox_camera.xacro

@@ -45,7 +45,7 @@
             </clip>
           </camera>
           <always_on>1</always_on>
-          <update_rate>50</update_rate>
+          <update_rate>20</update_rate>
           <visualize>false</visualize>
         </sensor>
     </gazebo>

src/dcaiti_control/description/colours.xacro

@@ -19,4 +19,16 @@
     <material name="red">
         <color rgba="1 0 0 1"/>
     </material>
+
+    <material name="gray">
+        <color rgba="0.753 0.753 0.753 1"/>
+    </material>
+
+    <material name="grey">
+        <color rgba="0.753 0.753 0.753 1"/>
+    </material>
+
+    <material name="silver">
+        <color rgba="0.88 0.88 0.88 1"/>
+    </material>
 </robot>

src/dcaiti_control/description/lidar.xacro

@@ -6,7 +6,7 @@
     <joint name="laser_joint" type="fixed">
         <parent link="chassis"/>
         <child link="laser_frame"/>
-        <origin xyz="0.1 0 0.175" rpy="0 0 0"/>
+        <origin xyz="0.1 0 0.35" rpy="0 0 0"/>
     </joint>
 
     
@@ -31,31 +31,37 @@
  
     <gazebo reference="laser_frame">
         <material>Gazebo/Red</material>
- 
+        <sensor name='gpu_lidar' type='gpu_lidar'>
-        <sensor name="laser" type="ray">
+          <topic>lidar</topic>
-            <pose> 0 0 0 0 0 0 </pose>
+          <update_rate>10</update_rate>
-            <visualize>false</visualize>
+          <ignition_frame_id>laser_frame</ignition_frame_id>
-            <update_rate>10</update_rate>
+          <ray>
-            <ray>
+            <scan>
-                <scan>
+              <horizontal>
-                    <horizontal>
+                <samples>640</samples>
-                        <samples>360*5</samples>
+                <resolution>1</resolution>
-                        <min_angle>-3.14</min_angle>
+                <min_angle>-1.396263</min_angle>
-                        <max_angle>3.14</max_angle>
+                <max_angle>1.396263</max_angle>
-                    </horizontal>
+              </horizontal>
-                </scan>
+              <vertical>
-                <range>
+                <samples>128</samples>
-                    <min>0.3</min>
+                <resolution>1</resolution>
-                    <max>100</max>
+                <min_angle>${-10*deg_to_rad}</min_angle>
-                </range>
+                <max_angle>${10*deg_to_rad}</max_angle>
-            </ray>
+              </vertical>
-            <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
+            </scan>
-                <ros>
+            <range>
-                    <argument>~/out:=scan</argument>
+              <min>0.08</min>
-                </ros>
+              <max>60.0</max>
-                <output_type>sensor_msgs/LaserScan</output_type>
+              <resolution>0.01</resolution>
-                <frame_name>laser_frame</frame_name>
+            </range>
-            </plugin>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </ray>
+          <alwaysOn>true</alwaysOn>
         </sensor>
     </gazebo>  
 </robot>

src/dcaiti_control/description/robot.urdf.xacro

@@ -36,8 +36,8 @@
     <xacro:property name="steer_mass" value="1" />
 
     <!-- Axle positions -->
-    <xacro:property name="axle_offset" value="-0.5" />
+    <xacro:property name="axle_offset" value="0.5" />
-    <xacro:property name="steer_offset" value="-0.5" />
+    <xacro:property name="steer_offset" value="0.5" />
 
 
 
@@ -55,5 +55,7 @@
     </xacro:unless>
 
     <xacro:include filename="robot_core.xacro" />
+    <xacro:include filename="bbox_camera.xacro" />
+    <xacro:include filename="lidar.xacro" />
 
 </robot>

src/dcaiti_control/description/robot_core.xacro

@@ -18,17 +18,17 @@
         <visual>
             <origin xyz="0 0 0.035"/>
             <geometry>
-                <box size="0.265 0.175 0.07"/>
+                <box size="${base_length} ${base_width} 0.07"/>
             </geometry>
             <material name="white"/>
         </visual>
         <collision>
             <origin xyz="0 0 0.035"/>
             <geometry>
-                <box size="0.265 0.175 0.07"/>
+                <box size="${base_length} ${base_width} 0.07"/>
             </geometry>
         </collision>
-        <xacro:inertial_box mass="3.3" x="0.265" y="0.175" z="0.07">
+        <xacro:inertial_box mass="3.3" x="${base_length}" y="${base_width}" z="0.07">
             <origin xyz="0 0 0.035" rpy="0 0 0"/>
         </xacro:inertial_box>
     </link>
@@ -38,28 +38,6 @@
     </gazebo>
 
 
-    <!-- Front steer and rear wheel joints required for ackermann_steering_controller -->
-  <xacro:front_steer
-    name="front"
-    parent="base"
-    steer_radius="${steer_radius}"
-    steer_thickness="${steer_thickness}" 
-    steer_mass="${steer_mass}" 
-    base_length="${base_length}"
-    base_width="${base_width}"
-    axle_offset="${axle_offset}"
-    steer_height="${wheel_radius+steer_offset}">
-  </xacro:front_steer>
-
-  <xacro:rear_wheel
-    name="rear"
-    parent="base"
-    wheel_radius="${wheel_radius/4}"
-    wheel_thickness="${wheel_thickness/2}"
-    wheel_mass="${wheel_mass/32}">
-    <origin xyz="${-base_length/2+axle_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
-  </xacro:rear_wheel>
-
   <!-- Steerable front wheels -->
   <xacro:front_wheel_lr 
     name="front_right"

src/dcaiti_control/description/steerbot.urdf.xacro

@@ -1,158 +0,0 @@
-<?xml version="1.0"?>
-<!--
-  Robot model adapted from https://github.com/CIR-KIT/steer_drive_ros/blob/kinetic-devel/steer_drive_controller/test/common/urdf/steerbot.xacro
-
-  Modifications:
-    Remove base_footprint link. Useful for navigation but not necessary for a simple example.
-    Added materials for URDF
-    Updated inertials
-
-  Geometry:
-    The Ackermann steering controllers require the longitudinal
-    separation between the front and back wheel axes and the
-    lateral separation between the left and right front steering axes.
-    
-    For this model:
-    wheel_separation_h = base_length - 2 * axle_offset = 0.4
-    wheel_separation_w = base_width  + 2 * axle_offset = 0.4
--->
-<robot name="steer_bot" xmlns:xacro="http://wiki.ros.org/xacro">
-  <!-- Include xacro for inertials, materials and wheels -->
-  <xacro:include filename="wheel.xacro"/>
-  <xacro:include filename="inertial_macros.xacro"/>
-
-  <xacro:property name="robot_namespace" value="/steer_bot"/>
-
-  <!-- Gazebo plugins -->
-  <gazebo>
-    <!-- Load ros_control plugin using the steer_bot_hardware_gazebo
-    implementation of the hardware_interface::RobotHW -->
-    <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-      <robotNamespace>${robot_namespace}</robotNamespace>
-    </plugin>
-  </gazebo>
-
-  <!-- Math constants -->
-  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
-
-  <!-- Robot base dimensions -->
-  <xacro:property name="base_length" value="10.0" /> 
-  <xacro:property name="base_width" value="10.0" /> 
-  <xacro:property name="base_height" value="0.1" /> 
-  <xacro:property name="base_mass" value="5.0" /> 
-
-  <!-- Wheel link dimensions -->
-  <xacro:property name="wheel_radius" value="0.1" />
-  <xacro:property name="wheel_thickness" value="0.08" />
-  <xacro:property name="wheel_mass" value="1" />
-
-  <!-- Steering link dimensions -->
-  <xacro:property name="steer_radius" value="0.05" />
-  <xacro:property name="steer_thickness" value="0.02" />
-  <xacro:property name="steer_mass" value="1" />
-
-  <!-- Axle positions -->
-  <xacro:property name="axle_offset" value="-0.5" />
-  <xacro:property name="steer_offset" value="-0.5" />
-
-  <!-- Base link -->
-  <link name="base_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="1 1 1"/>
-      </geometry>
-      <material name="green" />
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="1 1 1"/>
-      </geometry>
-    </collision>
-    <xacro:solid_cuboid_inertial 
-        rpy="0 0 0" xyz="0 0 0"
-        mass="${base_mass}"
-        x="${base_length}" y="${base_width}" z="${base_height}" />
-  </link>
-
-  <!-- Front steer and rear wheel joints required for ackermann_steering_controller -->
-  <xacro:front_steer
-    name="front"
-    parent="base"
-    steer_radius="${steer_radius}"
-    steer_thickness="${steer_thickness}" 
-    steer_mass="${steer_mass}" 
-    base_length="${base_length}"
-    base_width="${base_width}"
-    axle_offset="${axle_offset}"
-    steer_height="${wheel_radius+steer_offset}">
-  </xacro:front_steer>
-
-  <xacro:rear_wheel
-    name="rear"
-    parent="base"
-    wheel_radius="${wheel_radius/4}"
-    wheel_thickness="${wheel_thickness/2}"
-    wheel_mass="${wheel_mass/32}">
-    <origin xyz="${-base_length/2+axle_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
-  </xacro:rear_wheel>
-
-  <!-- Steerable front wheels -->
-  <xacro:front_wheel_lr 
-    name="front_right"
-    parent="base"
-    reflect="-1"
-    wheel_radius="${wheel_radius}"
-    wheel_thickness="${wheel_thickness}" 
-    wheel_mass="${wheel_mass}" 
-    steer_radius="${steer_radius}"
-    steer_thickness="${steer_thickness}" 
-    steer_mass="${steer_mass}" 
-    base_length="${base_length}"
-    base_width="${base_width}"
-    axle_offset="${axle_offset}"
-    steer_height="${wheel_radius+steer_offset}">
-  </xacro:front_wheel_lr>
-  <xacro:front_wheel_lr
-    name="front_left"
-    parent="base"
-    reflect="1"
-    wheel_radius="${wheel_radius}"
-    wheel_thickness="${wheel_thickness}"
-    wheel_mass="${wheel_mass}" 
-    steer_radius="${steer_radius}"
-    steer_thickness="${steer_thickness}" 
-    steer_mass="${steer_mass}" 
-    base_length="${base_length}"
-    base_width="${base_width}"
-    axle_offset="${axle_offset}"
-    steer_height="${wheel_radius+steer_offset}">
-  </xacro:front_wheel_lr>
-
-  <!-- Rear wheels -->
-  <xacro:rear_wheel_lr
-    name="rear_right"
-    parent="base"
-    reflect="-1"
-    wheel_radius="${wheel_radius}"
-    wheel_thickness="${wheel_thickness}"
-    wheel_mass="${wheel_mass}">
-    <origin xyz="${-base_length/2+axle_offset} ${-base_width/2-axle_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
-  </xacro:rear_wheel_lr>
-  <xacro:rear_wheel_lr
-    name="rear_left"
-    parent="base"
-    reflect="1"
-    wheel_radius="${wheel_radius}"
-    wheel_thickness="${wheel_thickness}"
-    wheel_mass="${wheel_mass}">
-    <origin xyz="${-base_length/2+axle_offset} ${+base_width/2+axle_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
-  </xacro:rear_wheel_lr>
-
-  <!-- Colour -->
-  <gazebo reference="base_link">
-    <material>Gazebo/Green</material>
-  </gazebo>
-
-</robot>