Skip to content
Snippets Groups Projects
Verified Commit 86baf6b7 authored by Anton Tetov Johansson's avatar Anton Tetov Johansson
Browse files

adapted to ilon_car_ctrl

parent 7f797134
No related branches found
No related tags found
No related merge requests found
*.pyc
.vscode/
......@@ -3,22 +3,23 @@ from __future__ import division
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int16MultiArray
from ilon_wheel_ctrl.msg import WheelRates
from mecanum_drive import controller
class ControllerNode:
ZERO_RATES = WheelRates(frontLeft=0, frontRight=0, backLeft=0, backRight=0)
def __init__(self):
self.controller = controller.Controller()
self.linearXVelocity = 0.0
self.linearYVelocity = 0.0
self.angularVelocity = 0.0
self.wheels_to_send = Int16MultiArray()
self.wheels_to_send = WheelRates()
def main(self):
self.wheelPub = rospy.Publisher('wheels_desired_rate',
Int16MultiArray, queue_size=1)
WheelRates, queue_size=1)
rospy.init_node('mecanum_drive_controller')
self.nodeName = rospy.get_name()
......@@ -49,11 +50,14 @@ class ControllerNode:
speeds = self.controller.getSpeeds(self.linearXVelocity,self.linearYVelocity,
self.angularVelocity)
self.wheels_to_send.data = [int(speeds.frontLeft), int(speeds.frontRight), int(speeds.rearLeft), int(speeds.rearRight)]
self.wheels_to_send.frontLeft = int(speeds.frontLeft)
self.wheels_to_send.frontRight = int(speeds.frontRight)
self.wheels_to_send.backLeft = int(speeds.rearLeft)
self.wheels_to_send.backRight = int(speeds.rearRight)
self.wheelPub.publish(self.wheels_to_send)
else:
self.wheels_to_send.data = [0, 0, 0, 0]
self.wheelPub.publish(self.wheels_to_send)
self.wheelPub.publish(self.ZERO_RATES)
def twistCallback(self, twist):
self.linearXVelocity = twist.linear.x
......
......@@ -17,6 +17,8 @@
<depend>geometry_msgs</depend>
<depend>actionlib_msgs</depend>
<build_depend>rostest</build_depend>
<build_depend>ilon_wheel_cmd</build_depend>
<depend>message_generation</depend>
<exec_depend>ilon_wheel_cmd</exec_depend>
</package>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment