From f2aedf52bb14074b837aa516e17b4330d7876901 Mon Sep 17 00:00:00 2001
From: daviddudas <david.dudas@outlook.com>
Date: Wed, 15 Jan 2020 17:24:05 +0100
Subject: [PATCH] wheel separation length parameter added
---
launch/demo.launch | 50 ++++-----------------------------
launch/revvy-config.launch | 42 ---------------------------
nodes/mecanum_drive_controller | 2 ++
nodes/mecanum_drive_odometry | 2 ++
src/mecanum_drive/controller.py | 9 ++++--
src/mecanum_drive/odometry.py | 3 ++
6 files changed, 18 insertions(+), 90 deletions(-)
delete mode 100755 launch/revvy-config.launch
diff --git a/launch/demo.launch b/launch/demo.launch
index 60711d5..48b1480 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 39ab4be..0000000
--- 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 d788a3a..f6be0a7 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 74c05ac..a7546bf 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 c7f8be4..28b290d 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 5e36026..bfb621d 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
--
GitLab