I started to practice with python, and I am getting a few errors that I does not understand very well.
This is my code:
#!/usr/bin/python
import rospy
import roslib
# Messages
from std_msgs.msg import Float32
from std_msgs.msg import Bool
# Issue commands to the GoPiGo motors to achieve the target velocity
# Use a PID that compares the error based on encoder readings
class ControlsToMotors:
def __init__(self):
rospy.init_node('surp_controller')
self.rate = rospy.get_param('~rate', 50)
# Wheel can turn ~17 ticks per second which is approx 5.34 rad / s when motor_cmd = 255 ******Modify - calibrate according to the Stepper motor
self.motor_max_angular_vel = rospy.get_param('~motor_max_angular_vel',5.32)
# Wheel can turn ~6 ticks per second which is approx 5.34 rad / s when motor_cmd = 125 ******Modify - calibrate according to the Stepper motor
self.motor_min_angular_vel = rospy.get_param('~motor_min_angular_vel',1.28)
# Corresponding motor commands
self.motor_cmd_max = rospy.get_param('~motor_cmd_max',255)
self.motor_cmd_min = rospy.get_param('~motor_cmd_min',110)
self.R = rospy.get_param('~robot_wheel_radius', 0.03)
self.gopigo_on = rospy.get_param('~gopigo_on',False)
if self.gopigo_on:
import gopigo
import atexit
atexit.register(gopigo.stop)
# (Optional) Publish the computed angular velocity targets
self.lwheel_angular_vel_target_pub = rospy.Publisher('lwheel_angular_vel_target', Float32, queue_size=10)
self.rwheel_angular_vel_target_pub = rospy.Publisher('rwheel_angular_vel_target', Float32, queue_size=10)
# (Optional) Publish the computed angular velocity control command
self.lwheel_angular_vel_control_pub = rospy.Publisher('lwheel_angular_vel_control', Float32, queue_size=10)
self.rwheel_angular_vel_control_pub = rospy.Publisher('rwheel_angular_vel_control', Float32, queue_size=10)
# (Optional) Publish the computed angular velocity motor command
self.lwheel_angular_vel_motor_pub = rospy.Publisher('lwheel_angular_vel_motor', Float32, queue_size=10)
self.rwheel_angular_vel_motor_pub = rospy.Publisher('rwheel_angular_vel_motor', Float32, queue_size=10)
# Publish commands to the arduino ***************************************
self.motor1_cmd_pub = rospy.Publisher("motor1_cmd", Float32, queue_size=10)
self.motor2_cmd_pub = rospy.Publisher("motor2_cmd", Float32, queue_size=10)
# Publish motor direction to robot******************************
self.motor1_dir_pub = rospy.Publisher("motor1_dir", bool, queue_size=10)
self.motor2_dir_pub = rospy.Publisher("motor2_dir", bool, queue_size=10)
# Read in encoders for PID control
self.lwheel_angular_vel_enc_sub = rospy.Subscriber('lwheel_angular_vel_enc', Float32, self.lwheel_angular_vel_enc_callback)
self.rwheel_angular_vel_enc_sub = rospy.Subscriber('rwheel_angular_vel_enc', Float32, self.rwheel_angular_vel_enc_callback)
# Read in tangential velocity targets
self.lwheel_tangent_vel_target_sub = rospy.Subscriber('lwheel_tangent_vel_target', Float32, self.lwheel_tangent_vel_target_callback)
self.rwheel_tangent_vel_target_sub = rospy.Subscriber('rwheel_tangent_vel_target', Float32, self.rwheel_tangent_vel_target_callback)
# Tangential velocity target
self.lwheel_tangent_vel_target = 0;
self.rwheel_tangent_vel_target = 0;
# Angular velocity target
self.lwheel_angular_vel_target = 0
self.rwheel_angular_vel_target = 0
# Angular velocity encoder readings
self.lwheel_angular_vel_enc = 0
self.rwheel_angular_vel_enc = 0
# motor_cmd I dont know if it is necessary
self.motor1_cmd = 0
self.motor2_cmd = 0
self.motor1_dir = True
self.motor2_dir = True
# ==================================================
# Publish motor cmd to the robot
# ==================================================
def motor1_cmd_pub(self, msg):
self.motor1_cmd = msg.data
def motor2_cmd_pub(self, msg):
self.motor2_cmd = msg.data
def motor1_dir_pub(self, msg):
self.motor1_dir = msg.data
def motor2_dir_pub(self, msg):
self.motor2_dir = msg.data
# ==================================================
# Read in tangential velocity targets
# ==================================================
def lwheel_tangent_vel_target_callback(self, msg):
self.lwheel_tangent_vel_target = msg.data
def rwheel_tangent_vel_target_callback(self, msg):
self.rwheel_tangent_vel_target = msg.data
# ==================================================
# Read in encoder readings for PID
# Consider for future iterations- modify for Back EMF, or use motor with encoder, or rely only on IMU
# ==================================================
def lwheel_angular_vel_enc_callback(self, msg):
self.lwheel_angular_vel_enc = msg.data
def rwheel_angular_vel_enc_callback(self, msg):
self.rwheel_angular_vel_enc = msg.data
# ==================================================
# Update motor commands
# ==================================================
# Compute angular velocity target
def tangentvel_2_angularvel(self,tangent_vel):
# v = wr
# v - tangential velocity (m/s)
# w - angular velocity (rad/s)
# r - radius of wheel (m)
angular_vel = tangent_vel / self.R;
return angular_vel
# Mapping angular velocity targets to motor commands
# Note: motor commands are ints between 0 - 255
# We also assume motor commands are issues between motor_min_angular_vel and motor_max_angular_vel
def angularvel_2_motorcmd(self, angular_vel_target):
if angular_vel_target == 0: return 0;
slope = (self.motor_cmd_max - self.motor_cmd_min) / (self.motor_max_angular_vel - self.motor_min_angular_vel)
intercept = self.motor_cmd_max - slope * self.motor_max_angular_vel
if angular_vel_target > 0: # positive angular velocity
motor_cmd = slope * angular_vel_target + intercept
if motor_cmd > self.motor_cmd_max: motor_cmd = self.motor_cmd_max
if motor_cmd < self.motor_cmd_min: motor_cmd = self.motor_cmd_min
else: # negative angular velocity
motor_cmd = slope * abs(angular_vel_target) + intercept
if motor_cmd > self.motor_cmd_max: motor_cmd = self.motor_cmd_max
if motor_cmd < self.motor_cmd_min: motor_cmd = self.motor_cmd_min
motor_cmd = -motor_cmd
return motor_cmd
# Send motor command to robot
# motor1 for left wheel. motor1(0, ?) tells wheel to move backwards. motor1(1, ?) tells wheel to move forwards
# motor2 for right wheel.
def motorcmd_2_robot(self, wheel='left', motor_command=0):
if self.gopigo_on:
motor_command_raw = int(abs(motor_command))
if wheel == 'left':
if motor_command >= 0:
self.motor1_cmd = motor_command_raw #use a publisher
self.motor1_dir = True
elif motor_command < 0:
self.motor1_cmd = motor_command_raw #use a publisher
self.motor1_dir = False #I had to split in 2 publisher so it stay simple
if wheel == 'right':
if motor_command >= 0:
self.motor2_cmd = motor_command_raw #use a publisher
self.motor2_dir = True
elif motor_command < 0:
self.motor2_cmd = motor_command_raw #use a publisher
self.motor2_dir = False
def lwheel_update(self):
# Compute target angular velocity
self.lwheel_angular_vel_target = self.tangentvel_2_angularvel(self.lwheel_tangent_vel_target)
self.lwheel_angular_vel_target_pub.publish(self.lwheel_angular_vel_target)
self.lwheel_angular_vel_control_pub.publish(self.lwheel_angular_vel_target)
# Compute motor command
lwheel_motor_cmd = self.angularvel_2_motorcmd(self.lwheel_angular_vel_target)
self.lwheel_angular_vel_motor_pub.publish(lwheel_motor_cmd)
# Send motor command
self.motorcmd_2_robot('left',lwheel_motor_cmd)
def rwheel_update(self):
# Compute target angular velocity
self.rwheel_angular_vel_target = self.tangentvel_2_angularvel(self.rwheel_tangent_vel_target)
self.rwheel_angular_vel_target_pub.publish(self.rwheel_angular_vel_target)
self.rwheel_angular_vel_control_pub.publish(self.rwheel_angular_vel_target)
# Compute motor command
rwheel_motor_cmd = self.angularvel_2_motorcmd(self.rwheel_angular_vel_target)
self.rwheel_angular_vel_motor_pub.publish(rwheel_motor_cmd)
# Send motor command
self.motorcmd_2_robot('right',rwheel_motor_cmd)
#motor cmd publish
def motor1_update(self):
self.motor1_cmd_pub.publish(self.motor1_cmd)
self.motor1_dir_pub.publish(self.motor1_dir)
def motor2_update(self):
self.motor2_cmd_pub.publish(self.motor2_cmd)
self.motor1_dir_pub.publish(self.motor1_dir)
# When given no commands for some time, do not move
def spin(self):
rospy.loginfo("Start surp_controller")
rate = rospy.Rate(self.rate)
rospy.on_shutdown(self.shutdown)
while not rospy.is_shutdown():
self.rwheel_update()
self.lwheel_update()
rate.sleep()
rospy.spin();
def shutdown(self):
rospy.loginfo("Stop surp_controller")
# Stop message
self.lwheel_angular_vel_target_pub.publish(0)
self.rwheel_angular_vel_target_pub.publish(0)
self.lwheel_angular_vel_control_pub.publish(0)
self.rwheel_angular_vel_control_pub.publish(0)
self.lwheel_angular_vel_motor_pub.publish(0)
self.rwheel_angular_vel_motor_pub.publish(0)
rospy.sleep(1)
def main():
controls_to_motors = ControlsToMotors();
controls_to_motors.spin()
if __name__ == '__main__':
main();
When I try to run my code called: the surp_controller.py
surp_controller.py is completely based on gopigo_controller.py .
I receive the following errors:
File "/home/rmb/gopigo_ws/src/gopigo_controller/src/surp_controller.py", line 230, in <module>
main();
File "/home/rmb/gopigo_ws/src/gopigo_controller/src/surp_controller.py", line 226, in main
controls_to_motors = ControlsToMotors();
File "/home/rmb/gopigo_ws/src/gopigo_controller/src/surp_controller.py", line 47, in __init__
self.motor1_dir_pub = rospy.Publisher("motor1_dir", bool, queue_size=10)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 842, in __init__
super(Publisher, self).__init__(name, data_class, Registration.PUB)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 144, in __init__
raise ValueError("data_class [%s] is not a message data class"%data_class.__class__.__name__)
ValueError: data_class [type] is not a message data class
Originally posted by renanmb on ROS Answers with karma: 33 on 2017-07-30
Post score: 0
