wheel separation length parameter added

launch/demo.launch

@@ -2,67 +2,27 @@
 
   <arg name="ticks_per_meter" value="10000" />
   <arg name="wheel_separation" value="0.2" />
+  <arg name="wheel_separation_length" value="0.3" />
 
-  <arg name="urdf_file" default="$(find diff_drive)/urdf/diff_drive_description.xacro" />
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf_file)" />
-
-  <!-- Publish the robot state -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher"
-        type="robot_state_publisher">
-    <param name="publish_frequency" value="10.0"/>
-  </node>
-
-  <!-- Provide simulated control of the robot joint angles -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher"
-        type="joint_state_publisher">
-    <param name="use_gui" value="False" />
-    <param name="rate" value="10.0"/>
-  </node>
-
-  <node name="controller" pkg="diff_drive" type="diff_drive_controller"
+  <node name="controller" pkg="mecanum_drive" type="mecanum_drive_controller"
         output="screen">
     <rosparam subst_value="true">
       ticks_per_meter: $(arg ticks_per_meter)
       wheel_separation: $(arg wheel_separation)
+      wheel_separation_length: $(arg wheel_separation_length)
       max_motor_speed: 3000
       timeout: 1.0
     </rosparam>
     <remap from="cmd_vel" to="/robot/cmd_vel" />
   </node>
   
-  <node name="odom_publisher" pkg="diff_drive" type="diff_drive_odometry"
+  <node name="odom_publisher" pkg="mecanum_drive" type="mecanum_drive_odometry"
         output="screen">
     <rosparam subst_value="true">
       ticks_per_meter: $(arg ticks_per_meter)
       wheel_separation: $(arg wheel_separation)
+      wheel_separation_length: $(arg wheel_separation_length)
     </rosparam>
   </node>
 
-  <node name="robot" pkg="diff_drive" type="diff_drive_mock_robot"
-        output="screen">
-    <remap from="~lwheel_desired_rate" to="lwheel_desired_rate" />
-    <remap from="~rwheel_desired_rate" to="rwheel_desired_rate" />
-    <remap from="~lwheel_ticks" to="lwheel_ticks" />
-    <remap from="~rwheel_ticks" to="rwheel_ticks" />
-  </node>
-
-  <node name="diff_drive_go_to_goal" pkg="diff_drive"
-        type="diff_drive_go_to_goal" output="screen">
-    <param name="~rate" value="20" />
-    <param name="~kP" value="0.5" />
-    <param name="~kA" value="1.0" />
-    <param name="~kB" value="-0.8" />
-    <param name="~max_linear_speed" value="0.2" />
-    <param name="~min_linear_speed" value="0.05" />
-    <param name="~max_angular_speed" value="0.7" />
-    <param name="~min_angular_speed" value="0.1" />
-    <param name="~linear_tolerance" value="0.01" />
-    <param name="~angular_tolerance" value="0.04" />
-    <param name="~forwardMovementOnly" value="false" />
-    <remap from="cmd_vel" to="/robot/cmd_vel" />
-  </node>
-
-  <node name="rviz" pkg="rviz" type="rviz"
-        args="-d $(find diff_drive)/config/view.rviz" />
-
 </launch>

launch/revvy-config.launch

-<?xml version="1.0"?>
-<launch>
-
-  <arg name="ticks_per_meter" value="1800" />
-  <arg name="wheel_separation" value="0.1" />
-
-  <arg name="urdf_file" default="$(find diff_drive)/urdf/diff_drive_description.xacro" />
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf_file)" />
-
-  <!-- Publish the robot state -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher"
-        type="robot_state_publisher">
-    <param name="publish_frequency" value="10.0"/>
-  </node>
-
-  <!-- Provide simulated control of the robot joint angles -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher"
-        type="joint_state_publisher">
-    <param name="use_gui" value="False" />
-    <param name="rate" value="10.0"/>
-  </node>
-
-  <node name="controller" pkg="diff_drive" type="diff_drive_controller"
-        output="screen">
-    <rosparam subst_value="true">
-      ticks_per_meter: $(arg ticks_per_meter)
-      wheel_separation: $(arg wheel_separation)
-      max_motor_speed: 3000
-      timeout: 1.0
-    </rosparam>
-  </node>
-  
-  <node name="odom_publisher" pkg="diff_drive" type="diff_drive_odometry"
-        output="screen">
-    <rosparam subst_value="true">
-      ticks_per_meter: $(arg ticks_per_meter)
-      wheel_separation: $(arg wheel_separation)
-    </rosparam>
-  </node>
-
-
-</launch>

nodes/mecanum_drive_controller

@@ -28,11 +28,13 @@ class ControllerNode:
 
         self.ticksPerMeter = float(rospy.get_param('~ticks_per_meter'))
         self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
+        self.wheelSeparationLength = float(rospy.get_param('~wheel_separation_length'))
         self.maxMotorSpeed = int(rospy.get_param('~max_motor_speed'))
         self.rate = float(rospy.get_param('~rate', 10.0))
         self.timeout = float(rospy.get_param('~timeout', 0.2))
 
         self.controller.setWheelSeparation(self.wheelSeparation)
+        self.controller.setWheelSeparationLength(self.wheelSeparationLength)
         self.controller.setTicksPerMeter(self.ticksPerMeter)
         self.controller.setMaxMotorSpeed(self.maxMotorSpeed)
 

nodes/mecanum_drive_odometry

@@ -47,6 +47,7 @@ class OdometryNode:
 
         self.ticksPerMeter = int(rospy.get_param('~ticks_per_meter'))
         self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
+        self.wheelSeparationLength = float(rospy.get_param('~wheel_separation_length'))
         self.rate = float(rospy.get_param('~rate', 20.0))
         self.baseFrameID = rospy.get_param('~base_frame_id', 'base_link')
         self.odomFrameID = rospy.get_param('~odom_frame_id', 'odom')
@@ -54,6 +55,7 @@ class OdometryNode:
         self.encoderMax = int(rospy.get_param('~encoder_max', 32767))
 
         self.odometry.setWheelSeparation(self.wheelSeparation)
+        self.odometry.setWheelSeparationLength(self.wheelSeparationLength)
         self.odometry.setTicksPerMeter(self.ticksPerMeter)
         self.odometry.setEncoderRange(self.encoderMin, self.encoderMax)
         self.odometry.setTime(rospy.get_time())

src/mecanum_drive/controller.py

@@ -25,8 +25,8 @@ class Controller:
     
         # print(linearXSpeed,linearYSpeed,angularSpeed)
     
-        WHEEL_SEPARATION_WIDTH = 0.1
-        WHEEL_SEPARATION_LENGTH = 0.13
+        WHEEL_SEPARATION_WIDTH = self.wheelSeparation
+        WHEEL_SEPARATION_LENGTH = self.wheelSeparationLength
         
         speeds = MotorCommand()
         
@@ -55,6 +55,9 @@ class Controller:
     def setWheelSeparation(self, separation):
         self.wheelSeparation = separation 
         
+    def setWheelSeparationLength(self, separation):
+        self.wheelSeparationLength = separation
+
     def setMaxMotorSpeed(self, limit):
         self.maxMotorSpeed = limit
 

src/mecanum_drive/odometry.py

@@ -17,6 +17,9 @@ class Odometry:
     def setWheelSeparation(self, separation):
         self.wheelSeparation = separation
 
+    def setWheelSeparationLength(self, separation):
+        self.wheelSeparationLength = separation
+
     def setTicksPerMeter(self, ticks):
         self.ticksPerMeter = ticks