So I have the follow code that allows my Robot to move via Teleop. I now need to Publish odometry values. Does anyone have any good ideas on how to do it?
import roslib
import rospy
from BrickPi import *
from std_msgs.msg import String
from std_msgs.msg import UInt16
from geometry_msgs.msg import Twist
#from nav_msgs.msg import *
#Model dependend settings
PI=3.141
ROBOT_WIDTH=0.16
WHEEL_DIAMETER=0.055
WHEEL_RADIUS=WHEEL_DIAMETER/2
WHEEL_PERIMETER=2*PI*WHEEL_RADIUS
#RPM dependant from voltage without load
#9.0V = 120 RPM
#7.5V = 80 RPM
# with load 60-80 rpm is a good average
MAX_RPM=80.0
RPS=MAX_RPM/60.0
MPS=RPS*WHEEL_PERIMETER
PWRDIV=1000*RPS
rospy.loginfo("PWRDIV:"+str(PWRDIV))
RF_WHEEL=PORT_C
LF_WHEEL=PORT_D
BrickPiSetup()
BrickPi.MotorEnable[RF_WHEEL] = 1
BrickPi.MotorEnable[LF_WHEEL] = 1
BrickPi.SensorType[PORT_1] = TYPE_SENSOR_ULTRASONIC_CONT
BrickPiSetupSensors()
def cmd_vel_callback(cmd_vel):
left_speed_out = cmd_vel.linear.x - cmd_vel.angular.z*ROBOT_WIDTH/2
right_speed_out = cmd_vel.linear.x + cmd_vel.angular.z*ROBOT_WIDTH/2
v = cmd_vel.linear.x # speed m/s
theta = cmd_vel.angular.z # angle rad/s
rospy.loginfo("VEL_CMD_CB: v:" + str(v) + ", theta:" + str(theta))
motor_control(left_speed_out, right_speed_out)
def vel_cmd_listener():
rospy.Subscriber("cmd_vel", Twist, cmd_vel_callback)
def motor_control(left_speed_out,right_speed_out):
rospy.loginfo("LSPEED:"+ str(left_speed_out) + " RSPEED:" + str(right_speed_out))
rospy.loginfo("LSPEED:"+ str(left_speed_out) + " RSPEED:" + str(right_speed_out))
BrickPi.MotorSpeed[RF_WHEEL] = int(-right_speed_out*PWRDIV)
rospy.loginfo("RF:"+str(BrickPi.MotorSpeed[RF_WHEEL]))
BrickPi.MotorSpeed[LF_WHEEL] = int(-left_speed_out*PWRDIV)
rospy.loginfo("LF:"+str(BrickPi.MotorSpeed[LF_WHEEL]))
BrickPiUpdateValues()
time.sleep(.01)
scan_publisher()
def scan_publisher():
result = BrickPiUpdateValues()
if not result :
range = BrickPi.Sensor[PORT_1]
us = rospy.Publisher('scan', UInt16)
us.publish(UInt16(range))
rospy.loginfo ("SCAN:" + str(range))
if __name__ == '__main__':
try:
rospy.init_node('RobotBaseController')
vel_cmd_listener()
time.sleep(.01)
rospy.spin()
except rospy.ROSInterruptException: pass
↧