diff --git a/launch/demo.launch b/launch/demo.launch
index 60711d53224ea1a1b42680d332a897122523aef8..48b1480b6900d707fc468d050df5da670b6bed9f 100755
--- a/launch/demo.launch
+++ b/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>
diff --git a/launch/revvy-config.launch b/launch/revvy-config.launch
deleted file mode 100755
index 39ab4befdad8ccfdd5b6228b3817567be368e9eb..0000000000000000000000000000000000000000
--- a/launch/revvy-config.launch
+++ /dev/null
@@ -1,42 +0,0 @@
-<?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>
diff --git a/nodes/mecanum_drive_controller b/nodes/mecanum_drive_controller
index d788a3a99bbab788a3072276e58201e9822ee826..f6be0a78e765fef099ae7a81f6e0acbd48379d1f 100755
--- a/nodes/mecanum_drive_controller
+++ b/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)
diff --git a/nodes/mecanum_drive_odometry b/nodes/mecanum_drive_odometry
index 74c05acdf94e21a525144426e5daf03c296ab5b0..a7546bff3ad6a0c36901263ffb9640580be93ecc 100755
--- a/nodes/mecanum_drive_odometry
+++ b/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())
diff --git a/src/mecanum_drive/controller.py b/src/mecanum_drive/controller.py
index c7f8be49b3d6b5a9a468b679e6373aee1fa8ef46..28b290dab02591843455345187dba81c8c447723 100755
--- a/src/mecanum_drive/controller.py
+++ b/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()
@@ -53,7 +53,10 @@ class Controller:
return speeds
def setWheelSeparation(self, separation):
- self.wheelSeparation = separation
+ self.wheelSeparation = separation
+
+ def setWheelSeparationLength(self, separation):
+ self.wheelSeparationLength = separation
def setMaxMotorSpeed(self, limit):
self.maxMotorSpeed = limit
diff --git a/src/mecanum_drive/odometry.py b/src/mecanum_drive/odometry.py
index 5e36026423cfc54d2e014924fb066392098b218b..bfb621d90cb6d17816ebac43a8054d6b58b14ac8 100755
--- a/src/mecanum_drive/odometry.py
+++ b/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