def main(portName, simulated):
print "INITIALIZING MOBILE BASE BY MARCOSOFT..."
###Connection with ROS
rospy.init_node("mobile_base")
pubOdometry = rospy.Publisher("mobile_base/odometry",
Odometry,
queue_size=1)
pubBattery = rospy.Publisher("robot_state/base_battery",
Float32,
queue_size=1)
subStop = rospy.Subscriber("robot_state/stop", Empty, callbackStop)
subSpeeds = rospy.Subscriber("mobile_base/speeds", Float32MultiArray,
callbackSpeeds)
#subCmdVel = rospy.Subscriber("mobile_base/cmd_vel", Twist, callbackCmdVel)
br = tf.TransformBroadcaster()
rate = rospy.Rate(20)
Roboclaw = roboclaw.Roboclaw(
portName, 115200) #nombre del puerto, baudrate del puerto
###Communication with the Roboclaw
if not simulated:
print "MobileBase.-> Trying to open serial port on \"" + portName + "\""
Roboclaw.Open()
address = 0x80
print "MobileBase.-> Serial port openned on \"" + portName + "\" at 38400 bps (Y)"
print "MobileBase.-> Clearing previous encoders readings"
Roboclaw.ResetEncoders(address) #reseteando encoders
###Variables for setting tire speeds
global leftSpeed
global rightSpeed
global newSpeedData
leftSpeed = 0
rightSpeed = 0
newSpeedData = False
speedCounter = 5
###Variables for odometry
robotPos = [0, 0, 0]
while not rospy.is_shutdown():
if newSpeedData:
newSpeedData = False
speedCounter = 5
if not simulated:
leftSpeed = int(leftSpeed * 127)
rightSpeed = int(rightSpeed * 127)
try:
if leftSpeed >= 0:
Roboclaw.ForwardM2(address, leftSpeed)
else:
Roboclaw.BackwardM2(address, -leftSpeed)
if rightSpeed >= 0:
Roboclaw.ForwardM1(address, rightSpeed)
else:
Roboclaw.BackwardM1(address, -rightSpeed)
except:
print "MOBILE BASE.->Error while sending speeds to Roboclaw"
else:
speedCounter -= 1
if speedCounter == 0:
if not simulated:
Roboclaw.ForwardM1(address, 0)
Roboclaw.ForwardM2(address, 0)
else:
leftSpeed = 0
rightSpeed = 0
if speedCounter < -1:
speedCounter = -1
if not simulated:
try:
c1, encoderLeft, c2 = Roboclaw.ReadEncM2(address)
d1, encoderRight, d2 = Roboclaw.ReadEncM1(address)
Roboclaw.ResetEncoders(address)
except:
print "MOBILE BASE.->Error while reading encoders"
else:
encoderLeft = leftSpeed * 0.1 * 16000 / 0.58
encoderRight = rightSpeed * 0.1 * 16000 / 0.58 ###Odometry calculation
robotPos = calculateOdometry(robotPos, encoderLeft, encoderRight)
#print "Encoders: " + str(encoderLeft) + " " + str(encoderRight)
##Odometry and transformations
ts = TransformStamped()
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = "odom"
ts.child_frame_id = "base_link"
ts.transform.translation.x = robotPos[0]
ts.transform.translation.y = robotPos[1]
ts.transform.translation.z = 0
ts.transform.rotation = tf.transformations.quaternion_from_euler(
0, 0, robotPos[2])
br.sendTransform((robotPos[0], robotPos[1], 0), ts.transform.rotation,
rospy.Time.now(), ts.child_frame_id,
ts.header.frame_id)
msgOdom = Odometry()
msgOdom.header.stamp = rospy.Time.now()
msgOdom.pose.pose.position.x = robotPos[0]
msgOdom.pose.pose.position.y = robotPos[1]
msgOdom.pose.pose.position.z = 0
msgOdom.pose.pose.orientation.x = 0
msgOdom.pose.pose.orientation.y = 0
msgOdom.pose.pose.orientation.z = math.sin(robotPos[2] / 2)
msgOdom.pose.pose.orientation.w = math.cos(robotPos[2] / 2)
pubOdometry.publish(msgOdom)
###Reads battery and publishes the corresponding topic
motorBattery = 18.5
if not simulated:
motorBattery = Roboclaw.ReadMainBatteryVoltage(
address) #Lectura de voltaje de bateria
#msgBattery = Float32()
#msgBattery.data = motorBattery
#pubBattery.publish(msgBattery)
rate.sleep()
#End of while
if not simulated:
Roboclaw.ForwardM1(address, 0)
Roboclaw.ForwardM2(address, 0)
import serial, time, sys, math
import rospy
from std_msgs.msg import Empty
from std_msgs.msg import Float32
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Odometry
from std_msgs.msg import Bool
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import Twist
from hardware_tools import roboclaw
import tf
base_diameter = 0.52;
rc_address_frontal = 0x80;
rc_address_lateral = 0x80;
rc_frontal = roboclaw.Roboclaw("/dev/ttyACM0", 38400); #Roboclaw controling motors for frontal movement (left and right)
rc_lateral = roboclaw.Roboclaw("/dev/ttyACM1", 38400); #Roboclaw controling motors for lateral movement (front and rear)
rc_acceleration = 1000000;
global simul;
simul = False;
def print_help():
print "MOBILE BASE BY MARCOSOFT. Options:"
print "\t --port \t Serial port name. If not provided, the default value is \"/dev/ttyACM0\""
print " - Mobile base can be moved by publishing either mobile_base/cmd_vel or"
print " - mobile_base/speeds. Speeds must be values in [-1, 1] where a value of 1 "
print " - represents the maximum speed that each motor can generate."
print "PLEASE DON'T TRY TO OPERATE JUSTINA IF YOU ARE NOT QUALIFIED ENOUGH."
def check_speed_ranges(s_left, s_right, s_front, s_rear): #speeds: left, right, front and rear
max_value_frontal = max(abs(s_left), abs(s_right));
###ALREADY IMPLEMENTED IN THE ROBOCLAW BOARD. THE SIMULATION OPTION HAS BEEN SUPRESSED
###FOR SIMULATION YOU SHOULD USE THE OMNI_BASE_SIMUL NODE.
import serial, time, sys, math
import rospy
from std_msgs.msg import Empty
from std_msgs.msg import Float32
from std_msgs.msg import Float32MultiArray
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import Twist
from hardware_tools import roboclaw
import tf
base_diameter = 0.86
rc_address_left = 0x80
rc_address_right = 0x80
rc_left = roboclaw.Roboclaw("/dev/ttyACM0", 38400)
#Roboclaw controling motors for frontal movement (left and right)
rc_right = roboclaw.Roboclaw("/dev/ttyACM1", 38400)
#Roboclaw controling motors for lateral movement (front and rear)
rc_acceleration = 1000000
def print_help():
print "MOBILE BASE BY MARCOSOFT. Options:"
print "\t --port \t Serial port name. If not provided, the default value is \"/dev/ttyACM0\""
print " - Mobile base can be moved by publishing either mobile_base/cmd_vel or"
print " - mobile_base/speeds. Speeds must be values in [-1, 1] where a value of 1 "
print " - represents the maximum speed that each motor can generate."
print "PLEASE DON'T TRY TO OPERATE JUSTINA IF YOU ARE NOT QUALIFIED ENOUGH."
def __init__(self):
print "MobileBase.-> INITIALIZING THE AMAZING MOBILE BASE NODE BY REY..."
rospy.init_node("mobile_base")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Trying to Connect to roboclaws")
port_name_frontal = "/dev/ttyACM0"
port_name_lateral = "/dev/ttyACM1"
self.simul = False
if rospy.has_param('~simul'):
self.simul = rospy.get_param('~simul')
if rospy.has_param('~port1'):
port_name_frontal = rospy.get_param('~port1')
elif not self.simul:
print_help()
sys.exit()
if rospy.has_param('~port2'):
port_name_lateral = rospy.get_param('~port2')
elif not self.simul:
#TODO put the correct config ros param help
print_help()
sys.exit()
#TODO If is necessary add the ros param address roboclaw
self.rc_address_frontal = 0x80
self.rc_address_lateral = 0x80
#self.BASE_WIDTH = 0.6
#388 + 6.5
self.BASE_WIDTH = 0.44
#self.TICKS_PER_METER_LATERAL = 103072.0 #Ticks per meter for the slow motors (front and rear)
#self.TICKS_PER_METER_FRONTAL = 103072.0 #Ticks per meter for the fast motors (left and right)
self.TICKS_PER_METER_LATERAL = 139071.0 #Ticks per meter for the slow motors (front and rear)
self.TICKS_PER_METER_FRONTAL = 139071.0 #Ticks per meter for the fast motors (left and right)
#baud_rate_frontal = 38400
#baud_rate_lateral = 38400
baud_rate_frontal = 115200
baud_rate_lateral = 115200
self.speed_front_left = 0
self.speed_front_right = 0
self.speed_rear = 0
if not self.simul:
self.rc_frontal = roboclaw.Roboclaw(port_name_frontal,
baud_rate_frontal)
#Roboclaw controling motors for frontal movement (left and right)
self.rc_lateral = roboclaw.Roboclaw(port_name_lateral,
baud_rate_lateral)
#Roboclaw controling motors for lateral movement (front and rear)
if self.rc_address_frontal > 0x87 or self.rc_address_frontal < 0x80:
rospy.logfatal("Address frontal out of range")
rospy.signal_shutdown("Address lateral out of range")
if self.rc_address_frontal > 0x87 or self.rc_address_frontal < 0x80:
rospy.logfatal("Address frontal out of range")
rospy.signal_shutdown("Address lateral out of range")
try:
self.rc_frontal.Open()
except Exception as e:
rospy.logfatal("Could not connect to frontal Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to frontal Roboclaw")
try:
self.rc_lateral.Open()
except Exception as e:
rospy.logfatal("Could not connect to lateral Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to lateral Roboclaw")
#TODO Config the diagnostic config
try:
frontal_version = self.rc_frontal.ReadVersion(
self.rc_address_frontal)
except Exception as e:
rospy.logwarn("Problem getting roboclaw frontal version")
rospy.logdebug(e)
pass
if not frontal_version[0]:
rospy.logwarn("Could not get version from frontal roboclaw")
else:
rospy.logdebug(repr(frontal_version[1]))
try:
lateral_version = self.rc_frontal.ReadVersion(
self.rc_address_lateral)
except Exception as e:
rospy.logwarn("Problem getting roboclaw lateral version")
rospy.logdebug(e)
pass
if not lateral_version[0]:
rospy.logwarn("Could not get version from lateral roboclaw")
else:
rospy.logdebug(repr(lateral_version[1]))
self.rc_frontal.SpeedM1M2(self.rc_address_frontal, 0, 0)
self.rc_frontal.ResetEncoders(self.rc_address_frontal)
self.rc_lateral.SpeedM1(self.rc_address_lateral, 0)
self.rc_lateral.ResetEncoders(self.rc_address_lateral)
#ROBOCLAW CONFIGURATION CONSTANT
pos_PID_front_left = self.rc_frontal.ReadM1PositionPID(
self.rc_address_frontal)
pos_PID_front_right = self.rc_frontal.ReadM2PositionPID(
self.rc_address_frontal)
pos_PID_rear = self.rc_lateral.ReadM1PositionPID(
self.rc_address_lateral)
vel_PID_front_left = self.rc_frontal.ReadM1VelocityPID(
self.rc_address_frontal)
vel_PID_front_right = self.rc_frontal.ReadM2VelocityPID(
self.rc_address_frontal)
vel_PID_rear = self.rc_lateral.ReadM1VelocityPID(
self.rc_address_lateral)
print "MobileBase.->Position PID constants: Success P I D MaxI Deadzone MinPos MaxPos"
print "MobileBase.->Front Left Motor: " + str(pos_PID_front_left)
print "MobileBase.->Front Right Motor: " + str(pos_PID_front_right)
print "MobileBase.->Rear Motor: " + str(pos_PID_rear)
print "MobileBase.->Velocity PID constants: Success P I D QPPS" #QPPS = speed in ticks/s when motor is at full speed
print "MobileBase.->Front Left Motor: " + str(vel_PID_front_left)
print "MobileBase.->Front Right Motor: " + str(vel_PID_front_right)
print "MobileBase.->Rear Motor: " + str(vel_PID_rear)
self.QPPS_FRONT_LEFT = vel_PID_front_left[4]
self.QPPS_FRONT_RIGHT = vel_PID_front_right[4]
self.QPPS_REAR = vel_PID_rear[4]
if self.QPPS_FRONT_LEFT != self.QPPS_FRONT_RIGHT or self.QPPS_FRONT_RIGHT != self.QPPS_REAR:
print "MobileBase.->WARNING: Motors have different max speeds!! Stall condition may occur."
if self.rc_frontal.ReadPWMMode(self.rc_address_frontal)[1] == 1:
print "MobileBase.->PWM Mode for left and right motors: Sign magnitude"
else:
print "MobileBase.->PWM Mode for left and right motors: Locked antiphase"
if self.rc_lateral.ReadPWMMode(self.rc_address_lateral)[1] == 1:
print "MobileBase.->PWM Mode for front and rear motors: Sign magnitude"
else:
print "MobileBase.->PWM Mode for front and rear motors: Locked antiphase"
else:
self.QPPS_FRONT_LEFT = self.TICKS_PER_METER_FRONTAL
self.QPPS_FRONT_RIGHT = self.TICKS_PER_METER_FRONTAL
self.QPPS_REAR = self.TICKS_PER_METER_LATERAL
self.encodm = EncoderOdom(self.TICKS_PER_METER_FRONTAL,
self.TICKS_PER_METER_LATERAL,
self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
rospy.sleep(1)
self.pubBattery = rospy.Publisher("mobile_base/base_battery",
Float32,
queue_size=1)
self.pubJointStates = rospy.Publisher("/joint_states",
JointState,
queue_size=1)
#subStop = rospy.Subscriber("robot_state/stop", Empty, callback_stop, queue_size=1);
self.subCmdVel = rospy.Subscriber("/hardware/mobile_base/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=1)
self.subSimul = rospy.Subscriber("/simulated",
Bool,
self.callback_simulated,
queue_size=1)
def main(port_name_1, port_name_2):
print "MobileBase.-> INITIALIZING THE AMAZING MOBILE BASE NODE BY MARCOSOFT..."
#ROS CONNECTION
rospy.init_node("omni_mobile_base")
pubBattery = rospy.Publisher("mobile_base/base_battery",
Float32,
queue_size=1)
subStop = rospy.Subscriber("robot_state/stop", Empty, callback_stop)
subSpeeds = rospy.Subscriber("/hardware/mobile_base/speeds",
Float32MultiArray, callback_speeds)
subCmdVel = rospy.Subscriber("/hardware/mobile_base/cmd_vel", Twist,
callback_cmd_vel)
br = tf.TransformBroadcaster()
rate = rospy.Rate(30)
#ROBOCLAW CONNECTION
rc_frontal = roboclaw.Roboclaw(port_name_1, 250000)
rc_lateral = roboclaw.Roboclaw(port_name_2, 250000)
rc_address1 = 0x80
rc_address2 = 0x80
if rc_frontal.Open() != 1:
print "MobileBase.-> Cannot open port " + port_name_1
return
#if rc_lateral.Open() != 1:
# print "MobileBase.-> Cannot open port " + port_name_2;
# return;
print "MobileBase.-> Roboclaw1 open on port " + port_name_1
print "MobileBase.-> Roboclaw2 open on port " + port_name_2
rc_frontal.ResetEncoders(rc_address1)
#rc_lateral.ResetEncoders(rc_address2);
#ROBOCLAW CONFIGURATION CONSTANTS
m_left_pos_PID = rc_frontal.ReadM1PositionPID(rc_address1)
m_right_pos_PID = rc_frontal.ReadM2PositionPID(rc_address1)
#m_front_pos_PID = rc_lateral.ReadM1PositionPID(rc_address2);
#m_rear_pos_PID = rc_lateral.ReadM2PositionPID(rc_address2);
m_left_vel_PID = rc_frontal.ReadM1VelocityPID(rc_address1)
m_right_vel_PID = rc_frontal.ReadM2VelocityPID(rc_address1)
#m_front_vel_PID = rc_lateral.ReadM1VelocityPID(rc_address2);
#m_rear_vel_PID = rc_lateral.ReadM2VelocityPID(rc_address2);
print "MobileBase.->Position PID constants: Success P I D MaxI Deadzone MinPos MaxPos"
print "MobileBase.->Left Motor: " + str(m_left_pos_PID)
print "MobileBase.->Right Motor: " + str(m_right_pos_PID)
#print "MobileBase.->Front Motor: " + str(m_front_pos_PID);
#print "MobileBase.->Rear Motor: " + str(m_rear_pos_PID);
print "MobileBase.->Velocity PID constants: Success P I D QPPS"
#QPPS = speed in ticks/s when motor is at full speed
print "MobileBase.->Left Motor: " + str(m_left_vel_PID)
print "MobileBase.->Right Motor: " + str(m_right_vel_PID)
#print "MobileBase.->Front Motor: " + str(m_front_vel_PID);
#print "MobileBase.->Left Motor: " + str(m_left_vel_PID);
QPPS_M_LEFT = m_left_vel_PID[4]
QPPS_M_RIGHT = m_right_vel_PID[4]
QPPS_M_FRONT = 1000
#m_front_vel_PID[4];
QPPS_M_REAR = 1000
#m_rear_vel_PID[4];
if QPPS_M_LEFT != QPPS_M_RIGHT or QPPS_M_REAR != QPPS_M_FRONT:
print "MobileBase.->WARNING: Motors have different max speeds!! Stall condition may occur."
if rc_frontal.ReadPWMMode(rc_address1)[1] == 1:
print "MobileBase.->PWM Mode for left and right motors: Sign magnitude"
else:
print "MobileBase.->PWM Mode for left and right motors: Locked antiphase"
#if rc_lateral.ReadPWMMode(rc_address2) == 1:
# print "MobileBase.->PWM Mode for front and rear motors: Sign magnitude"
#else:
# print "MobileBase.->PWM Mode for front and rear motors: Locked antiphase"
#Variable initialization
M_LR_ACCELERATION = 15000
#Acceleration for speed control of left and right motors
M_FR_ACCELERATION = 30000
#Acceleration for speed control of front and rear motors
global new_data
global speed_left
global speed_right
global speed_front
global speed_rear
speed_left = 0
speed_right = 0
speed_front = 0
speed_rear = 0
new_data = False
no_new_data_counter = 10
#If no new speed data arrive after 5 cycles, motors are stopped.
robot_x = 0
robot_y = 0
robot_t = 0
encoder_left_last = 0
encoder_right_last = 0
encoder_front_last = 0
encoder_rear_last = 0
while not rospy.is_shutdown():
if new_data:
new_data = False
no_new_data_counter = 10
speed_left = int(speed_left * 16.0 / 35.0 * QPPS_M_LEFT)
#Factor 16/35 is due to the different motor reductions
speed_right = int(speed_right * 16.0 / 35.0 * QPPS_M_RIGHT)
#Factor 16/35 is due to the different motor reductions
speed_front = int(speed_front * QPPS_M_FRONT)
speed_rear = int(speed_rear * QPPS_M_REAR)
try:
rc_frontal.SpeedAccelM1M2(rc_address1, M_LR_ACCELERATION,
speed_left, speed_right)
#rc_lateral.SpeedAccelM1M2(rc_address2, M_FR_ACCELERATION, speed_front, speed_rear);
except:
print "MobileBase.->Error while sending speed to left-right Roboclaw"
else:
no_new_data_counter -= 1
if no_new_data_counter == 0:
rc_frontal.ForwardM1(rc_address1, 0)
rc_frontal.ForwardM2(rc_address1, 0)
#rc_lateral.ForwardM1(rc_address2, 0);
#rc_lateral.ForwardM2(rc_address2, 0);
if no_new_data_counter < -1:
no_new_data_counter = -1
encoder_left = rc_frontal.ReadEncM1(rc_address1)[1]
encoder_right = rc_frontal.ReadEncM2(rc_address1)[1]
encoder_front = 0
#rc_lateral.ReadEncM1(rc_address2)[1];
encoder_rear = 0
#rc_lateral.ReadEncM2(rc_address2)[1];
delta_left = encoder_left - encoder_left_last
delta_right = encoder_right - encoder_right_last
delta_front = encoder_front - encoder_front_last
delta_rear = encoder_rear - encoder_rear_last
encoder_left_last = encoder_left
encoder_right_last = encoder_right
encoder_front_last = encoder_front
encoder_rear_last = encoder_rear
if (math.fabs(delta_left) < 10000 and math.fabs(delta_right) < 10000
and math.fabs(delta_front) < 10000
and math.fabs(delta_rear) < 10000):
(robot_x, robot_y,
robot_t) = calculate_odometry(robot_x, robot_y, robot_t,
delta_left, delta_right,
delta_front, delta_rear)
else:
print "MobileBase.->Invalid encoder readings."
quaternion = tf.transformations.quaternion_from_euler(0, 0, robot_t)
br.sendTransform((robot_x, robot_y, 0), quaternion, rospy.Time.now(),
"base_link", "odom")
pubBattery.publish(
Float32(rc_frontal.ReadMainBatteryVoltage(rc_address1)[1]))
rate.sleep()
print "MobileBase.->Stopping motors..."
rc_frontal.ForwardM1(rc_address1, 0)
rc_frontal.ForwardM2(rc_address1, 0)