class DummyViconNode(object):
"""
Example python node including reading parameters, publishers and subscribers, and timer.
"""
def __init__(self):
# Remember that init_node hasn't been called yet so only do stuff here that
# doesn't require node handles etc.
pass
def start(self):
rospy.init_node('python_node_example')
self.tfb = TransformBroadcaster()
self.init_params()
self.init_publishers()
self.init_timers()
rospy.spin()
def init_params(self):
pass
# self.param_one = rospy.get_param("~param_one", 100.0)
# self.param_two = rospy.get_param("~param_two", False)
def init_publishers(self):
self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
def init_timers(self):
self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
def vicon_timer_callback(self, event):
msg = TransformStamped()
msg.header.stamp = rospy.Time.now()
msg.transform.rotation.w = 1.0
self.pub.publish(msg)
self.tfb.sendTransform((0,0,0), (0,0,0,1), rospy.Time.now(), '/pelican1/flyer_vicon', '/enu')
class RTbotController:
def __init__(self, arduino):
self.arduino = arduino
self.stopped = False
# Subscribe to rtbot_motors
rospy.Subscriber("rtbot_motors", Int16, self.rtbotMotorsCallback)
# Setup the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
def poll(self):
(x, y, theta) = self.arduino.rtbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
now,
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
# Possible issue: If we were commanding the robot via Twist messages
# then we would have some sensible values to put here. But by setting
# the motor commands directly, we don't have access to plausible values
# for the forward and angular speeds. Does this even matter???
#odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.x = 0
odom.twist.twist.linear.y = 0
#odom.twist.twist.angular.z = self.angularSpeed
odom.twist.twist.angular.z = 0
self.odomPub.publish(odom)
def stop(self):
self.stopped = True
self.arduino.rtbot_set_motors(0)
def rtbotMotorsCallback(self, msg):
self.arduino.rtbot_set_motors(msg.data)
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("switch_dom")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.mapping_frame_id = rospy.get_param(
'~mapping_frame_id',
'odom_mapping') # the name of the base frame of the robot
self.nav_frame_id = rospy.get_param(
'~nav_frame_id',
'odom_nav') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param(
'~odom_frame_id',
'odom_comb') # the name of the odometry reference frame
self.odomPub = rospy.Publisher("odom_comb", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.__mode = RobotMode.STARTING
self.mode_sub_ = rospy.Subscriber('robot_mode', RobotMode,
self.mode_callback)
self.frame_id = self.nav_frame_id
self.update()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(1000)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def mode_callback(self, msg):
#############################################################################
if msg.code == RobotMode.MAPPING:
self.frame_id = self.mapping_frame_id
print 'mapping'
else:
self.frame_id = self.nav_frame_id
print msg.code
pass
#############################################################################
def update(self):
#############################################################################
# publish the odom information
self.odomBroadcaster.sendTransform((0, 0, 0), (0.0, 0.0, 0.0, 1.0),
rospy.Time.now(),
self.odom_frame_id, self.frame_id)
class RTbotController:
def __init__(self, arduino):
self.arduino = arduino
self.stopped = False
# Subscribe to rtbot_motors
rospy.Subscriber("rtbot_motors", Int16, self.rtbotMotorsCallback)
# Setup the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
def poll(self):
(x, y, theta) = self.arduino.rtbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), now,
"base_link", "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
# Possible issue: If we were commanding the robot via Twist messages
# then we would have some sensible values to put here. But by setting
# the motor commands directly, we don't have access to plausible values
# for the forward and angular speeds. Does this even matter???
#odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.x = 0
odom.twist.twist.linear.y = 0
#odom.twist.twist.angular.z = self.angularSpeed
odom.twist.twist.angular.z = 0
self.odomPub.publish(odom)
def stop(self):
self.stopped = True
self.arduino.rtbot_set_motors(0)
def rtbotMotorsCallback(self, msg):
self.arduino.rtbot_set_motors(msg.data)
class OdometryPub:
def __init__(self):
self.base_frame_id = 'base_link' # the name of the base frame of the robot
self.odom_frame_id = 'odom' # the name of the odometry reference frame
# internal data
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.then = 0
self.odomPub = rospy.Publisher("/odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
def update(self):
global x
global y
global vx
global vy
global heading
global heading_old
if (self.then == 0):
self.then = rospy.Time.now()
return
now = rospy.Time.now()
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(heading / 2)
quaternion.w = cos(heading / 2)
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = heading - heading_old
self.odomPub.publish(odom)
def start():
# ROS IMU 전달부 세팅 #
#static_transform = rospy.get_param('~static_transform', [0, 0, 0, 0, 0, 0])
fixed_frame = rospy.get_param('~fixed_frame', "base_footprint")
frame_name = rospy.get_param('~frame_name', "imu")
rospy.init_node('lsm303', anonymous=True)
pub = rospy.Publisher('imu', Imu, queue_size=1)
odomBroadcaster_imu = TransformBroadcaster()
frequency = 5
r = rospy.Rate(frequency)
data = Imu()
seq = 0
print("[lsm303] loop start!")
while not rospy.is_shutdown():
data.header.frame_id = "imu"
data.header.stamp = rospy.get_rostime()
data.header.stamp = rospy.Time.now()
data.header.seq = seq
data.linear_acceleration.x, data.linear_acceleration.y, data.linear_acceleration.z = accel.acceleration
#print("acceleration: {}".format(accel.acceleration))
mag_x, mag_y, mag_z = mag.magnetic
theta = -math.atan2(mag_y, mag_x)
#print("theta: {}".format(theta))
q = tf.transformations.quaternion_from_euler(0, 0, theta)
data.orientation.x = q[0]
data.orientation.y = q[1]
data.orientation.z = q[2]
data.orientation.w = q[3]
odomBroadcaster_imu.sendTransform((0, 0, 0), (0, 0, 0, 1),
rospy.Time.now(), frame_name,
fixed_frame)
#print("q(x,y,z,w): ({}, {}, {}, {})".format(data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w))
data.angular_velocity.x = 0
data.angular_velocity.y = 0
data.angular_velocity.z = 0
seq += 1
pub.publish(data)
r.sleep()
print("[lsm303] program end")
class odomTransformer():
def __init__(self):
# naming
rospy.init_node('Odom_Transformer', anonymous=False)
# broadcaster
self.tf_broadcaster = TransformBroadcaster()
# subscribe the odom topic
rospy.Subscriber('odom', Odometry, self.pub_odom_tf)
def pub_odom_tf(self, msg):
self.tf_broadcaster.sendTransform(
(msg.pose.pose.position.x, msg.pose.pose.position.y, 0),
(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w),
rospy.Time.now(), "base_footprint", "odom")
class DummyViconNode(object):
"""
Example python node including reading parameters, publishers and subscribers, and timer.
"""
def __init__(self):
# Remember that init_node hasn't been called yet so only do stuff here that
# doesn't require node handles etc.
pass
def start(self):
rospy.init_node('python_node_example')
self.tfb = TransformBroadcaster()
self.init_params()
self.init_publishers()
self.init_timers()
rospy.spin()
def init_params(self):
pass
self.tf_ref_frame_id = rospy.get_param("~tf_ref_frame_id", "/enu")
self.tf_tracked_frame_id = rospy.get_param("~tf_tracked_frame_id", "/pelican1/flyer_vicon")
self.dummy_position = rospy.get_param("~dummy_position", (0.0, -0.3593, -0.05))
self.dummy_orientation = rospy.get_param("~dummy_orientation", (0.5, 0.5, -0.5, 0.5)) # xyzw
self.enable_tf_broadcast = rospy.get_param("~enable_tf_broadcast", False)
# self.param_two = rospy.get_param("~param_two", False)
def init_publishers(self):
self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
def init_timers(self):
self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
def vicon_timer_callback(self, event):
msg = TransformStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_ref_frame_id
msg.child_frame_id = self.tf_tracked_frame_id
msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z = self.dummy_position
(msg.transform.rotation.x, msg.transform.rotation.y,
msg.transform.rotation.z, msg.transform.rotation.w) = self.dummy_orientation
self.pub.publish(msg)
if self.enable_tf_broadcast:
self.tfb.sendTransform(self.dummy_position, self.dummy_orientation,
rospy.Time.now(), self.tf_tracked_frame_id, self.tf_ref_frame_id)
class Hector_Pose_2_Odom():
def __init__(self):
# ros
rospy.Subscriber("/slam_out_pose", PoseStamped, self.slam_out_pose_cb)
#self.odomPub = rospy.Publisher("odom", Odometry, queue_size=3)
self.odomBroadcaster = TransformBroadcaster()
self.base_frame_id = "base_footprint"
self.odom_frame_id = "odom"
rospy.loginfo("Init Hector_Pose_2_TF_Odom Finish!")
def slam_out_pose_cb(self, req):
orientation = req.pose.orientation
self.odomBroadcaster.sendTransform(
(req.pose.position.x, req.pose.position.y, 0),
(orientation.x, orientation.y, orientation.z, orientation.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
class MockLocalization():
def __init__(self):
rospy.init_node("mock_localization")
nodename = rospy.get_name()
self.rate = rospy.get_param("~rate", 20)
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.poseCallback)
self.pose = Pose()
self.pose.position.x = 0
self.pose.position.y = 0
self.pose.position.z = 0
quat = tf.transformations.quaternion_from_euler(0,0,0)
self.pose.orientation.x = quat[0]
self.pose.orientation.y = quat[1]
self.pose.orientation.z = quat[2]
self.pose.orientation.w = quat[3]
self.broadcaster = TransformBroadcaster()
def spin(self):
r = rospy.Rate(self.rate)
while(not rospy.is_shutdown()):
self.broadcaster.sendTransform(
(self.pose.position.x, self.pose.position.y, self.pose.position.z),
(self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w),
rospy.Time.now(),
'odom',
'map'
)
r.sleep()
def poseCallback(self,msg):
self.pose.position.x = msg.pose.pose - self.pose.position.x
self.pose.position.y = msg.pose.pose - self.pose.position.y
self.pose.position.z = msg.pose.pose - self.pose.position.z
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.245)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.world_frame_id = rospy.get_param('~world_frame_id', 'world') # the name of the world reference frame
self.use_laser_scan_matcher = rospy.get_param("use_laser_scan_matcher", False)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# subscriptions
self.covariance = [
0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1
]
rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.listener = tf.TransformListener()
rospy.Subscriber("pose2D", Pose2D, self.pose_callback, queue_size=1)
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
def update_from_laser_scan_matcher(self):
if not self.listener.canTransform(self.base_frame_id, self.world_frame_id,
rospy.Time(0)):
rospy.logwarn("Transform not available %s -> %s", "/%s" % (self.base_frame_id), "/%s" % (self.world_frame_id))
return
(trans, rot) = self.listener.lookupTransform("/%s" % (self.base_frame_id), "/%s" % (self.world_frame_id), rospy.Time(0))
#rospy.loginfo("Fixing odom x: %f y: %f th: %f -> x: %f y %f (%s %s)", self.x, self.y, self.th, trans[0], trans[1], trans, rot)
self.x = trans[0]
self.y = trans[1]
self.th = asin(rot[2]) * 2
def pose_callback(self, data):
rospy.logdebug("Odom x: %f y: %f th: %f --- Pose x: %f y %f th: %f", self.x, self.y, self.th, data.x, data.y, data.theta)
if self.use_laser_scan_matcher:
self.x = data.x
self.y = data.y
self.th = data.theta
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
#if self.use_laser_scan_matcher:
# self.update_from_laser_scan_matcher()
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = ( d_left + d_right ) / 2
# this approximation works (in radians) for small angles
th = ( d_right - d_left ) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos( th ) * d
y = -sin( th ) * d
# calculate the final position of the robot
self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
if( th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin( self.th / 2 )
quaternion.w = cos( self.th / 2 )
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance = self.covariance
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
odom.twist.covariance = self.covariance
self.odomPub.publish(odom)
#############################################################################
def lwheelCallback(self, msg):
#############################################################################
enc = msg.data
if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap):
self.lmult = self.lmult + 1
if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap):
self.lmult = self.lmult - 1
self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min))
self.prev_lencoder = enc
#############################################################################
def rwheelCallback(self, msg):
#############################################################################
enc = msg.data
if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap):
self.rmult = self.rmult + 1
if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap):
self.rmult = self.rmult - 1
self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min))
self.prev_rencoder = enc
class OdomPub():
def __init__(self):
rospy.init_node("odom_pub")
nodename = rospy.get_name()
self.base_width = rospy.get_param("~base_width", 0.11)
self.ticks_meter = rospy.get_param("~ticks_meter", 1130)
self.base_frame_id = rospy.get_param("~base_frame_id", "base_link")
self.odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
self.rate = rospy.get_param("~rate", 20)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
rospy.Subscriber('lticks', Int32, self.lticksCallback)
rospy.Subscriber('rticks', Int32, self.rticksCallback)
self.lticks = 0
self.rticks = 0
self.lastticks_l = 0
self.lastticks_r = 0
self.x = 0
self.y = 0
self.th = 0
def spin(self):
r = rospy.Rate(self.rate)
self.last = rospy.Time.now()
while not rospy.is_shutdown():
self.spinOnce()
r.sleep()
def spinOnce(self):
now = rospy.Time.now()
dT = now - self.last
dT = dT.to_sec()
self.last = now
d_left = float(self.lticks - self.lastticks_l) / self.ticks_meter
d_right = float(self.rticks - self.lastticks_r) / self.ticks_meter
self.lastticks_l = self.lticks
self.lastticks_r = self.rticks
d = (d_left + d_right) / 2
th = (d_right - d_left) / self.base_width
self.dx = d / dT
self.dr = th / dT
if (d != 0):
# calculate distance traveled in x and y
x = cos(th) * d
y = -sin(th) * d
# calculate the final position of the robot
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), now,
self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
def lticksCallback(self, msg):
self.lticks = msg.data
pass
def rticksCallback(self, msg):
self.rticks = msg.data
pass
class HCRNode:
def __init__(self):
""" Start up connection to the HCR Robot. """
rospy.init_node('hcr')
self.port = rospy.get_param('~port', ARDUINO_PORT)
rospy.loginfo("Using port: %s" % (self.port))
self.robot = hcr(self.port)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = [0, 0]
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(5)
while not rospy.is_shutdown():
odom.header.stamp = rospy.Time.now()
# get motor velocity values
vr, vl = self.robot.getMotors()
# send updated movement commands
self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1])
# now update position information
dt = (odom.header.stamp - then).to_sec()
then = odom.header.stamp
#odometry navigation
omegaRight = vr / WHEELS_RAD
omegaLeft = vl / WHEELS_RAD
linear_velocity = (WHEELS_RAD / 2) * (omegaRight + omegaLeft)
angular_velocity = (WHEELS_RAD / WHEELS_DIST) * (omegaRight -
omegaLeft)
self.th += (angular_velocity * dt)
self.th = normalize_angle(self.th)
self.x += linear_velocity * cos(self.th) * dt
self.y += linear_velocity * sin(self.th) * dt
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
#odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = linear_velocity
odom.twist.twist.angular.z = angular_velocity
# publish everything
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
"base_link", "odom")
self.odomPub.publish(odom)
# wait, then do it again
r.sleep()
# shut down
self.robot.stop()
def cmdVelCb(self, req):
vLinear = req.linear.x
vAngular = req.angular.z
vr = ((2 * vLinear) + (WHEELS_DIST * vAngular)) / 2
vl = ((2 * vLinear) - (WHEELS_DIST * vAngular)) / 2
k = max(abs(vr), abs(vl))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
vr = vr * MAX_SPEED / k
vl = vl * MAX_SPEED / k
self.cmd_vel = [round(vr, 1), round(vl, 1)]
class BaseController:
def __init__(self, arduino):
self.arduino = arduino
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param('~base_controller_timeout', 1.0)
self.stopped = False
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param(
"~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Used to Determin which cmd_vel to listen to
self.cmdvel2 = False
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (
self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " +
str(self.wheel_track) + "m wide with " +
str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), "base_link", "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)
def cmdVelCallback(self, req):
#check if we are using cmd_vel2 at the moment
if self.cmdvel2:
return
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter /
self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter /
self.arduino.PID_RATE)
def cmdVel2Callback(self, req):
# Got a request, shut off cmd_vel
self.cmdvel2 = True
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# if we are not turning in place, then go back to cmd_vel
if th == 0:
self.cmdvel2 = False
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter /
self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter /
self.arduino.PID_RATE)
class BaseController:
def __init__(self, arduino, base_frame, name='base_controller'):
self.arduino = arduino
self.name = name
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
self.odom_linear_scale_correction = rospy.get_param("~odom_linear_scale_correction", 1.0)
self.odom_angular_scale_correction = rospy.get_param("~odom_angular_scale_correction", 1.0)
self.use_imu_heading = rospy.get_param("~use_imu_heading", False)
self.publish_odom_base_transform = rospy.get_param("~publish_odom_base_transform", True)
self.stopped = False
self.current_speed = Twist()
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 1.0)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
self.detect_enc_jump_error = rospy.get_param("~detect_enc_jump_error", False)
self.enc_jump_error_threshold = rospy.get_param("~enc_jump_error_threshold", 1000)
# Default error threshold (percent) before getting a diagnostics warning
self.base_diagnotics_error_threshold = rospy.get_param("~base_diagnotics_error_threshold", 10)
# Diagnostics update rate
self.base_diagnotics_rate = rospy.get_param("~base_diagnotics_rate", 1.0)
# Create the diagnostics updater for the Arduino device
self.diagnostics = DiagnosticsUpdater(self, self.name, self.base_diagnotics_error_threshold, self.base_diagnotics_rate)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# Internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# Subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] is None or pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
if self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko):
rospy.loginfo("PID parameters update to: Kp=%d, Kd=%d, Ki=%d, Ko=%d" %(self.Kp, self.Kd, self.Ki, self.Ko))
else:
rospy.logerr("Updating PID parameters failed!")
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
self.diagnostics.reads += 1
self.diagnostics.total_reads += 1
left_enc, right_enc = self.arduino.get_encoder_counts()
self.diagnostics.freq_diag.tick()
except:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
# Check for jumps in encoder readings
if self.detect_enc_jump_error:
try:
#rospy.loginfo("Left: %d LEFT: %d Right: %d RIGHT: %d", left_enc, self.enc_left, right_enc, self.enc_right)
enc_jump_error = False
if abs(right_enc - self.enc_right) > self.enc_jump_error_threshold:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("RIGHT encoder jump error from %d to %d", self.enc_right, right_enc)
self.enc_right = right_enc
enc_jump_error = True
if abs(left_enc - self.enc_left) > self.enc_jump_error_threshold:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("LEFT encoder jump error from %d to %d", self.enc_left, left_enc)
self.enc_left = left_enc
enc_jump_error = True
if enc_jump_error:
return
except:
pass
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = self.odom_linear_scale_correction * (dright + dleft) / 2.0
dth = self.odom_angular_scale_correction * (dright - dleft) / float(self.wheel_track)
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
if self.publish_odom_base_transform:
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame,
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.current_speed = Twist()
self.current_speed.linear.x = vxy
self.current_speed.angular.z = vth
"""
Covariance values taken from Kobuki node odometry.cpp at:
https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp
Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
Odometry yaw covariance must be much bigger than the covariance provided
by the imu, as the later takes much better measures
"""
odom.pose.covariance[0] = 0.1
odom.pose.covariance[7] = 0.1
if self.use_imu_heading:
#odom.pose.covariance[35] = 0.2
odom.pose.covariance[35] = 0.05
else:
odom.pose.covariance[35] = 0.05
odom.pose.covariance[14] = sys.float_info.max # set a non-zero covariance on unused
odom.pose.covariance[21] = sys.float_info.max # dimensions (z, pitch and roll); this
odom.pose.covariance[28] = sys.float_info.max # is a requirement of robot_pose_ekf
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)
def cmdVelCallback(self, req):
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
def reset_odometry(self):
self.x = 0.0
self.y = 0.0
self.th = 0.0
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 282.5)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.26)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.lowLevelInits()
# subscriptions
rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
rospy.Subscriber("Nav_State", UInt32, self.navState)
self.odomPub = rospy.Publisher("odom", Odometry)
self.odomBroadcaster = TransformBroadcaster()
def lowLevelInits(self):
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
def navState(self, data):
if data.data == 2:
self.lowLevelInits()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = ( d_left + d_right ) / 2
# this approximation works (in radians) for small angles
th = ( d_right - d_left ) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos( th ) * d
y = -sin( th ) * d
# calculate the final position of the robot
self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
if( th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin( self.th / 2 )
quaternion.w = cos( self.th / 2 )
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
self.odomBroadcaster.sendTransform((0,0,0.23), (-0.500, 0.485, -0.500, 0.515), rospy.Time.now(), "camera", "base_link")
#############################################################################
def lwheelCallback(self, msg):
#############################################################################
enc = msg.data
if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap):
self.lmult = self.lmult + 1
if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap):
self.lmult = self.lmult - 1
self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min))
self.prev_lencoder = enc
#############################################################################
def rwheelCallback(self, msg):
#############################################################################
enc = msg.data
if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap):
self.rmult = self.rmult + 1
if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap):
self.rmult = self.rmult - 1
self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min))
self.prev_rencoder = enc
class CreateDriver:
def __init__(self):
port = rospy.get_param('~port', "/dev/ttyUSB0")
self.create = Create(port)
self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
self.fields = [
'wheeldropCaster', 'wheeldropLeft', 'wheeldropRight', 'bumpLeft',
'bumpRight', 'wall', 'cliffLeft', 'cliffFronLeft',
'cliffFrontRight', 'cliffRight', 'virtualWall', 'infraredByte',
'advance', 'play', 'distance', 'angle', 'chargingState', 'voltage',
'current', 'batteryTemperature', 'batteryCharge',
'batteryCapacity', 'wallSignal', 'cliffLeftSignal',
'cliffFrontLeftSignal', 'cliffFrontRightSignal',
'cliffRightSignal', 'homeBase', 'internalCharger', 'songNumber',
'songPlaying', 'x', 'y', 'theta', 'chargeLevel'
]
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
self.create.update = self.sense
self.inDock = False
self.chargeLevel = 0
def start(self):
self.create.start()
self.then = datetime.now()
def stop(self):
self.create.stop()
def sense(self):
now = datetime.now()
elapsed = now - self.then
self.then = now
elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
d = self.create.d_distance / 1000.
th = self.create.d_angle * pi / 180
dx = d / elapsed
dth = th / elapsed
if (d != 0):
x = cos(th) * d
y = -sin(th) * d
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), "base_link", "odom")
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth
self.odomPub.publish(odom)
packet = SensorPacket()
for field in self.fields[:-4]:
packet.__setattr__(field, self.create.__getattr__(field))
self.chargeLevel = float(packet.batteryCharge) / float(
packet.batteryCapacity)
packet.__setattr__('x', self.x)
packet.__setattr__('y', self.y)
packet.__setattr__('theta', self.th)
packet.__setattr__('chargeLevel', self.chargeLevel)
self.packetPub.publish(packet)
if packet.homeBase:
self.inDock = True
else:
self.inDock = False
def circle(self, req):
self.create.forwardTurn(req.speed, req.radius)
return CircleResponse(True)
def demo(self, req):
self.create.demo(req.demo)
return DemoResponse(True)
def leds(self, req):
self.create.leds(req.advance, req.play, req.color, req.intensity)
return LedsResponse(True)
def tank(self, req):
self.create.tank(req.left, req.right)
return TankResponse(True)
def turn(self, req):
if (req.clear):
self.create.clear()
self.create.turn(req.turn)
return TurnResponse(True)
def dock(self, req):
""" req.charge = 0: dock and charge
= 1: wake up from charging
= 2: dock and wake up immediately """
if req.charge == 2:
self.create.restart()
return DockResponse(True)
self.create.brake()
self.create.demo(1)
while (not self.inDock):
pass
if req.charge == 0:
rospy.sleep(3)
self.create.restart()
return DockResponse(True)
""" Added summer 2012 """
def opCode(self, opCode):
self.create.send(opCode)
def twist(self, req):
x = req.linear.x * 1000.
th = req.angular.z
print(x, th)
if (x == 0):
th = th * 180 / pi
speed = (8 * pi * th) / 9
self.create.left(int(speed))
elif (th == 0):
x = int(x)
self.create.tank(x, x)
else:
self.create.forwardTurn(int(x), int(x / th))
def song(self, req):
self.create.playFullSong(req.notes, req.durations)
return SongResponse(True)
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
setSpeed(tcp=100, ori=30)
setZone(0)
# set the parameters
limit = 10000 # number of data points to be collected
ori = [0, 0.7071, 0.7071, 0]
threshold = 0.3 # the threshold force for contact, need to be tuned
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
step_size = 0.0002
obj_des_wrt_vicon = [0,0,-(9.40/2/1000+14.15/2/1000),0,0,0,1]
# visualize the block
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
# 0. move to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos,ori)
start_pos = [0.3, 0.06, z]
setCart(start_pos,ori)
curr_pos = start_pos
# 0.1 zero the ft reading
rospy.sleep(1)
setZero()
rospy.sleep(3)
# 1. move in -y direction till contact -> normal
setSpeed(tcp=30, ori=30)
direc = np.array([0, -step_size, 0])
normal = [0,0,0]
while True:
curr_pos = np.array(curr_pos) + direc
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = ftmsg2list(ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg())
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener)
# correct box_pose
box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon)))
print 'box_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
break
#pause()
# 2. use the normal to move along the block
setSpeed(tcp=20, ori=30)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 2.1 move
direc = np.dot(tfm.euler_matrix(0,0,2) , normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = getAveragedFT()
print index #, '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener)
# correct box_pose
box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon)))
#box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
vizBlock(obj_des_wrt_vicon)
br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7], rospy.Time.now(), "SteelBlockDesired", "map")
#print 'box_pos', box_pos, 'box_quat', box_quat
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radis
contact_pts.append(contact_pt.tolist())
contact_pt[2] = 0.01
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z. Also the normal is in toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] + (-normal).tolist()[0:2] + [0] + box_pose_des_global[0:3] + box_pose_des_global[6:7] + box_pose_des_global[3:6] + curr_pos.tolist())
index += 1
if len(contact_pts) > limit:
break
if len(contact_pts) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=5, ori=30)
setCart(curr_pos + normal * move_away_size, ori)
rospy.sleep(1)
print 'bad ft:', getAveragedFT()
setZero()
rospy.sleep(3)
setCart(curr_pos - normal * overshoot_size, ori)
setSpeed(tcp=20, ori=30)
#all_contact(1:2,:); % x,y of contact position
#all_contact(4:5,:); % x,y contact normal
#all_contact(7:9,:); % box x,y
#all_contact(10:13,:); % box quaternion
#all_contact(14:16,:); % pusher position
# save contact_nm and contact_pt as json file
with open(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.json', 'w') as outfile:
json.dump({'contact_pts': contact_pts, 'contact_nms': contact_nms}, outfile)
# save all_contact as mat file
sio.savemat(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.mat', {'all_contact': all_contact})
setSpeed(tcp=100, ori=30)
# 3. move back to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos,ori)
class ThymioDriver(object):
def frame_name(self, name):
if self.tf_prefix:
return '{self.tf_prefix}/{name}'.format(**locals())
return name
def change_config(self, config, level):
# self.diff_factor = config.diff_factor
# self.ticks2mm = config.ticks2mm
self.motor_speed_deadband = config.motor_speed_deadband
return config
@property
def motor_speed_deadband(self):
return self._motor_speed_deadband
@motor_speed_deadband.setter
def motor_speed_deadband(self, value):
self._motor_speed_deadband = value
self.odom_msg.twist.covariance[0] = speed_cov = 0.5 * (value / 1000 / SPEED_COEF) ** 2
self.odom_msg.twist.covariance[-1] = speed_cov / (self.axis ** 2)
def __init__(self):
rospy.init_node('thymio')
# load script on the Thymio
rospy.wait_for_service('aseba/get_node_list')
get_aseba_nodes = rospy.ServiceProxy(
'aseba/get_node_list', GetNodeList)
while True:
if 'thymio-II' in get_aseba_nodes().nodeList:
break
rospy.loginfo('Waiting for thymio node ...')
rospy.sleep(1)
rospy.wait_for_service('aseba/load_script')
load_script = rospy.ServiceProxy('aseba/load_script', LoadScripts)
default_script_path = os.path.join(
roslib.packages.get_pkg_dir('thymio_driver'), 'aseba/thymio_ros.aesl')
script_path = rospy.get_param('~script', default_script_path)
rospy.loginfo("Load aseba script %s", script_path)
load_script(script_path)
# initialize parameters
self.tf_prefix = rospy.get_param('tf_prefix', '')
self.odom_frame = self.frame_name(rospy.get_param('~odom_frame', 'odom'))
self.robot_frame = self.frame_name(rospy.get_param('~robot_frame', 'base_link'))
self.x = 0
self.y = 0
self.th = 0
self.then = rospy.Time.now()
odom_rate = rospy.get_param('~odom_max_rate', -1)
if odom_rate == 0:
self.odom_min_period = -1
else:
self.odom_min_period = 1.0 / odom_rate
self.odom_msg = Odometry(header=rospy.Header(frame_id=self.odom_frame),
child_frame_id=self.robot_frame)
self.odom_msg.pose.pose.position.z = 0
self.odom_msg.pose.covariance[0] = -1
self.odom_msg.header.stamp = rospy.Time.now()
# subscribe to topics
self.aseba_pub = rospy.Publisher(
'aseba/events/set_speed', AsebaEvent, queue_size=1)
self.left_wheel_angle = 0
self.right_wheel_angle = 0
self.axis = rospy.get_param('~axis_length', BASE_WIDTH / 1000.0)
self.motor_speed_deadband = rospy.get_param('~motor_speed_deadband', 10)
# self.ticks2mm = rospy.get_param('~ticks2mm', 1.0 / SPEED_COEF)
# self.diff_factor = rospy.get_param('~diff_factor', 1.0)
def_cal = [0.001 / SPEED_COEF, 0]
left_wheel_calibration = rospy.get_param('~left_wheel_calibration/q', def_cal)
self.left_wheel_speed, self.left_wheel_motor_speed = motor_speed_conversion(
*left_wheel_calibration)
rospy.loginfo('Init left wheel with calibration %s', left_wheel_calibration)
right_wheel_calibration = rospy.get_param('~right_wheel_calibration/q', def_cal)
self.right_wheel_speed, self.right_wheel_motor_speed = motor_speed_conversion(
*right_wheel_calibration)
rospy.loginfo('Init right wheel with calibration %s', right_wheel_calibration)
left_wheel_joint = rospy.get_param('~left_wheel_joint', 'left_wheel_joint')
right_wheel_joint = rospy.get_param('~right_wheel_joint', 'right_wheel_joint')
self.wheel_state_msg = JointState()
self.wheel_state_msg.name = [left_wheel_joint, right_wheel_joint]
self.wheel_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self.odom_broadcaster = TransformBroadcaster()
rospy.Subscriber('aseba/events/odometry', AsebaEvent, self.on_aseba_odometry_event)
rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)
self.buttons = Joy()
self.buttons_pub = rospy.Publisher('buttons', Joy, queue_size=1)
rospy.Subscriber("aseba/events/buttons", AsebaEvent, self.on_aseba_buttons_event)
for button in BUTTONS:
rospy.Subscriber('aseba/events/button_' + button,
AsebaEvent, self.on_aseba_button_event(button))
proximity_range_min = rospy.get_param('~proximity/range_min', 0.0215)
proximity_range_max = rospy.get_param('~proximity/range_min', 0.14)
proximity_field_of_view = rospy.get_param('~proximity/field_of_view', 0.3)
self.proximity_sensors = [{
'publisher': rospy.Publisher('proximity/' + name, Range, queue_size=1),
'msg': Range(
header=rospy.Header(
frame_id=self.frame_name('proximity_{name}_link'.format(name=name))),
radiation_type=Range.INFRARED,
field_of_view=proximity_field_of_view,
min_range=proximity_range_min,
max_range=proximity_range_max)}
for name in PROXIMITY_NAMES]
self.proximityToLaserPublisher = rospy.Publisher(
'proximity/laser', LaserScan, queue_size=1)
self.proximityToLaser = LaserScan(
header=rospy.Header(frame_id=self.frame_name('laser_link')),
angle_min=-0.64, angle_max=0.64, angle_increment=0.32,
time_increment=0, scan_time=0, range_min=proximity_range_min + 0.08,
range_max=proximity_range_max + 0.08)
rospy.Subscriber('aseba/events/proximity', AsebaEvent, self.on_aseba_proximity_event)
self.ground_sensors = [{
'publisher': rospy.Publisher('ground/' + name, Range, queue_size=1),
'msg': Range(
header=rospy.Header(
frame_id=self.frame_name('ground_{name}_link'.format(name=name))),
radiation_type=Range.INFRARED, field_of_view=proximity_field_of_view,
min_range=(GROUND_MIN_RANGE / 1000.0), max_range=(GROUND_MAX_RANGE / 1000.0))
} for name in GROUND_NAMES]
ground_threshold = rospy.get_param('~ground/threshold', 200)
rospy.Subscriber('aseba/events/ground', AsebaEvent, self.on_aseba_ground_event)
rospy.set_param("~ground_threshold", ground_threshold)
self.imu = Imu(header=rospy.Header(frame_id=self.robot_frame))
# no orientation or angular velocity information
self.imu.orientation_covariance[0] = -1
self.imu.angular_velocity_covariance[0] = -1
# just an accelerometer
self.imu.linear_acceleration_covariance[0] = 0.07
self.imu.linear_acceleration_covariance[4] = 0.07
self.imu.linear_acceleration_covariance[8] = 0.07
self.imu_publisher = rospy.Publisher('imu', Imu, queue_size=1)
rospy.Subscriber('aseba/events/accelerometer',
AsebaEvent, self.on_aseba_accelerometer_event)
self.tap_publisher = rospy.Publisher('tap', Empty, queue_size=1)
rospy.Subscriber('aseba/events/tap', AsebaEvent, self.on_aseba_tap_event)
self.temperature = Temperature(
header=rospy.Header(frame_id=self.robot_frame))
self.temperature.variance = 0.01
self.temperature_publisher = rospy.Publisher('temperature', Temperature, queue_size=1)
rospy.Subscriber('aseba/events/temperature', AsebaEvent, self.on_aseba_temperature_event)
self.sound_publisher = rospy.Publisher('sound', Float32, queue_size=1)
self.sound_threshold_publisher = rospy.Publisher(
'aseba/events/set_sound_threshold', AsebaEvent, queue_size=1)
rospy.Subscriber('aseba/events/sound', AsebaEvent, self.on_aseba_sound_event)
rospy.Subscriber('sound_threshold', Float32, self.on_sound_threshold)
self.remote_publisher = rospy.Publisher('remote', Int8, queue_size=1)
rospy.Subscriber('aseba/events/remote', AsebaEvent, self.on_aseba_remote_event)
rospy.Subscriber('comm/transmit', Int16, self.on_sound_threshold)
self.comm_publisher = rospy.Publisher('comm/receive', Int16, queue_size=1)
self.aseba_set_comm_publisher = rospy.Publisher(
'aseba/events/set_comm', AsebaEvent, queue_size=1)
rospy.Subscriber('aseba/events/comm', AsebaEvent, self.on_aseba_comm_event)
# actuators
for name in BODY_LEDS:
rospy.Subscriber('led/body/' + name, ColorRGBA, self.on_body_led(name))
rospy.Subscriber('led', Led, self.on_led)
self.aseba_led_publisher = rospy.Publisher(
'aseba/events/set_led', AsebaEvent, queue_size=6)
rospy.Subscriber('led/off', Empty, self.on_led_off)
rospy.Subscriber('led/gesture', LedGesture, self.on_led_gesture)
self.aseba_led_gesture_publisher = rospy.Publisher(
'aseba/events/set_led_gesture', AsebaEvent, queue_size=6)
rospy.Subscriber('led/gesture/circle', Float32, self.on_led_gesture_circle)
rospy.Subscriber('led/gesture/off', Empty, self.on_led_gesture_off)
rospy.Subscriber('led/gesture/blink', Float32, self.on_led_gesture_blink)
rospy.Subscriber('led/gesture/kit', Float32, self.on_led_gesture_kit)
rospy.Subscriber('led/gesture/alive', Empty, self.on_led_gesture_alive)
rospy.Subscriber('sound/play', Sound, self.on_sound_play)
self.aseba_led_gesture_publisher = rospy.Publisher(
'aseba/events/set_led_gesture', AsebaEvent, queue_size=6)
rospy.Subscriber('sound/play/system', SystemSound, self.on_system_sound_play)
self.aseba_play_sound_publisher = rospy.Publisher(
'aseba/events/play_sound', AsebaEvent, queue_size=1)
self.aseba_play_system_sound_publisher = rospy.Publisher(
'aseba/events/play_system_sound', AsebaEvent, queue_size=1)
rospy.Subscriber('alarm', Bool, self.on_alarm)
self.alarm_timer = None
rospy.Subscriber('shutdown', Empty, self.on_shutdown_msg)
self.aseba_shutdown_publisher = rospy.Publisher(
'aseba/events/shutdown', AsebaEvent, queue_size=1)
rospy.on_shutdown(self.shutdown)
Server(ThymioConfig, self.change_config)
# tell ros that we are ready
rospy.Service('thymio_is_ready', std_srvs.srv.Empty, self.ready)
def on_shutdown_msg(self, msg):
self.aseba_shutdown_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, []))
def ready(self, req):
return std_srvs.srv.Empty()
def play_system_sound(self, sound):
self.aseba_play_system_sound_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [sound]))
def alarm_cb(self, evt):
self.play_system_sound(2)
def on_alarm(self, msg):
if msg.data and not self.alarm_timer:
self.alarm_timer = rospy.Timer(rospy.Duration(3), self.alarm_cb)
if not msg.data and self.alarm_timer:
self.alarm_timer.shutdown()
self.alarm_timer = None
def on_sound_play(self, msg):
freq = max(1, int(msg.frequency))
duration = max(1, int(msg.duration.to_sec() * 60))
self.aseba_play_sound_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [freq, duration]))
def on_system_sound_play(self, msg):
self.play_system_sound(msg.sound)
def set_led_gesture(self, gesture, leds, wave, period, length, mirror, mask):
period = max(-32678, min(32678, int(period * 1000)))
data = [gesture, leds, wave, period, length, mirror] + mask[:8]
data += [1] * (14 - len(data))
self.aseba_led_gesture_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, data))
def on_led_gesture(self, msg):
self.set_led_gesture(msg.gesture, msg.leds, msg.wave,
msg.period, msg.length, msg.mirror, msg.mask)
def on_led_gesture_off(self, msg):
self.set_led_gesture(LedGesture.OFF, 0, 0, 0, 0, 0, [])
def on_led_gesture_circle(self, msg):
self.set_led_gesture(LedGesture.WAVE, LedGesture.CIRCLE,
LedGesture.HARMONIC, msg.data, 8, 0, [])
def on_led_gesture_blink(self, msg):
self.set_led_gesture(LedGesture.WAVE, LedGesture.CIRCLE,
LedGesture.HARMONIC, msg.data, 1, 0, [])
def on_led_gesture_kit(self, msg):
self.set_led_gesture(LedGesture.WAVE, LedGesture.PROXIMITY,
LedGesture.HARMONIC, msg.data, 12, 11, [1, 1, 1, 1, 1, 1, 0, 0])
def on_led_gesture_alive(self, msg):
self.set_led_gesture(LedGesture.WAVE, LedGesture.CIRCLE,
LedGesture.RECT, 3.0, 24, 0, [])
def on_led_off(self, msg):
for i in LED_NUMBER.keys():
self.aseba_led_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [i] + 8 * [0]))
# sleep to avoid that aseba or ros do not process all messages.
# could be improved by having 6 separate aseba topics where to send
# messages
rospy.sleep(0.005)
def on_led(self, msg):
i = msg.id
num = LED_NUMBER.get(i, 0)
if num <= len(msg.values):
data = [i] + [int(32 * v) for v in msg.values[:8]]
data += [0] * (9 - len(data))
self.aseba_led_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, data))
def on_body_led(self, name):
publisher = rospy.Publisher(
'aseba/events/set_led_' + name, AsebaEvent, queue_size=1)
def callback(msg):
r = int(msg.r * 32)
g = int(msg.g * 32)
b = int(msg.b * 32)
aseba_msg = AsebaEvent(rospy.get_rostime(), 0, [r, g, b])
publisher.publish(aseba_msg)
return callback
def on_aseba_comm_event(self, msg):
self.comm_publisher.publish(Int16(msg.data[0]))
def on_aseba_remote_event(self, msg):
self.remote_publisher.publish(Int8(msg.data[1]))
def on_sound_threshold(self, msg):
value = msg * 255
if value < 0:
value = 1
if value > 255:
value = 0
self.sound_threshold_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [value]))
def on_aseba_sound_event(self, msg):
self.sound_publisher.publish(Float32(msg.data[0] / 255.0))
def on_aseba_tap_event(self, msg):
self.tap_publisher.publish(Empty())
def on_aseba_temperature_event(self, msg):
self.temperature.temperature = msg.data[0] / 10.0
self.temperature_publisher.publish(self.temperature)
# TODO check how it's implemented in the firmware.
def on_aseba_accelerometer_event(self, msg):
self.imu.linear_acceleration.x = msg.data[1] / 23.0 * 9.81
self.imu.linear_acceleration.y = -msg.data[0] / 23.0 * 9.81
self.imu.linear_acceleration.z = msg.data[2] / 23.0 * 9.81
self.imu.header.stamp = rospy.Time.now()
self.imu_publisher.publish(self.imu)
def on_aseba_ground_event(self, msg):
data = msg.data
ir_threshold = rospy.get_param("~ground_threshold", 200)
for sensor, value in zip(self.ground_sensors, data):
sensor['msg'].range = float('inf') if (
value < ir_threshold) else -float('inf')
sensor['msg'].header.stamp = rospy.Time.now()
sensor['publisher'].publish(sensor['msg'])
# basics logarithmic fit
@staticmethod
def proximity2range(raw):
if raw > 4000:
return -float('inf')
if raw < 800:
return float('inf')
return -0.0736 * log(raw) + 0.632
def on_aseba_proximity_event(self, msg):
data = msg.data
values = [self.proximity2range(d) for d in data]
for sensor, value in zip(self.proximity_sensors, values):
sensor['msg'].range = value
sensor['msg'].header.stamp = rospy.Time.now()
sensor['publisher'].publish(sensor['msg'])
self.proximityToLaser.ranges = []
self.proximityToLaser.intensities = []
self.proximityToLaser.header.stamp = rospy.Time.now()
for dist, raw in zip(values, data)[4::-1]:
if dist > 0.14:
dist = 0.14
if dist < 0.0215:
dist = 0.0215
self.proximityToLaser.ranges.append(dist + 0.08)
self.proximityToLaser.intensities.append(raw)
self.proximityToLaserPublisher.publish(self.proximityToLaser)
def on_aseba_button_event(self, button):
publisher = rospy.Publisher('buttons/' + button, Bool, queue_size=1)
def callback(msg):
bool_msg = Bool(msg.data[0])
publisher.publish(bool_msg)
return callback
# ======== we send the speed to the aseba running on the robot ========
def set_speed(self, values):
self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(), 0, values))
# ======== stop the robot safely ========
def shutdown(self):
self.set_speed([0, 0])
def on_aseba_buttons_event(self, data):
self.buttons.header.stamp = rospy.Time.now()
self.buttons.buttons = data.data
self.buttons_pub.publish(self.buttons)
# ======== processing odometry events received from the robot ========
def on_aseba_odometry_event(self, data):
now = data.stamp
dt = (now - self.then).to_sec()
self.then = now
m_l, m_r = data.data
if abs(m_l) < self.motor_speed_deadband:
m_l = 0
if abs(m_r) < self.motor_speed_deadband:
m_r = 0
vl = self.left_wheel_speed(m_l)
vr = self.right_wheel_speed(m_r)
# wheel joint states
left_wheel_angular_speed = vl / WHEEL_RADIUS
right_wheel_angular_speed = vr / WHEEL_RADIUS
self.left_wheel_angle += dt * left_wheel_angular_speed
self.right_wheel_angle += dt * right_wheel_angular_speed
dsl = vl * dt # left wheel delta in m
dsr = vr * dt # right wheel delta in m
# robot traveled distance in meters
ds = ((dsl + dsr) / 2.0)
dth = (dsr - dsl) / self.axis # turn angle
self.x += ds * cos(self.th + dth / 2.0)
self.y += ds * sin(self.th + dth / 2.0)
self.th += dth
# We publish odometry, tf, and wheel joint state only at a maximal rate:
if self.odom_min_period > (now - self.odom_msg.header.stamp).to_sec():
return
self.wheel_state_msg.header.stamp = rospy.Time.now()
self.wheel_state_msg.position = [self.left_wheel_angle, self.right_wheel_angle]
self.wheel_state_msg.velocity = [left_wheel_angular_speed, right_wheel_angular_speed]
self.wheel_state_pub.publish(self.wheel_state_msg)
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
self.odom_msg.header.stamp = rospy.Time.now() # OR TO TAKE ONE FROM THE EVENT?
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
self.odom_msg.pose.pose.orientation = quaternion
if(dt > 0):
self.odom_msg.twist.twist.linear.x = ds / dt
self.odom_msg.twist.twist.angular.z = dth / dt
# publish odometry
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
self.then, self.robot_frame, self.odom_frame)
self.odom_pub.publish(self.odom_msg)
def set_linear_angular_speed(self, speed, ang_speed):
left_wheel_speed = speed - ang_speed * 0.5 * self.axis
right_wheel_speed = speed + ang_speed * 0.5 * self.axis
left_motor_speed = round(self.left_wheel_motor_speed(left_wheel_speed))
right_motor_speed = round(self.right_wheel_motor_speed(right_wheel_speed))
max_motor_speed = max(abs(left_motor_speed), abs(right_motor_speed))
if max_motor_speed > MAX_SPEED:
return self.set_linear_angular_speed(speed * MAX_SPEED / max_motor_speed,
ang_speed * MAX_SPEED / max_motor_speed)
self.set_speed([left_motor_speed, right_motor_speed])
def on_cmd_vel(self, data):
self.set_linear_angular_speed(data.linear.x, data.angular.z)
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.CMD_RATE =2
self.port = rospy.get_param('~port', "/dev/ttyUSB0")
rospy.loginfo("Using port: %s" % self.port)
self.robot = Botvac(self.port)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.buttonPub = rospy.Publisher('button', Button, queue_size=10)
self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = [0, 0]
self.old_vel = self.cmd_vel
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min =0.0
scan.angle_max =359.0*pi/180.0
scan.angle_increment =pi/180.0
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')
button = Button()
sensor = Sensor()
self.robot.setBacklight(1)
self.robot.setLED("Green")
# main loop of driver
r = rospy.Rate(20)
cmd_rate= self.CMD_RATE
while not rospy.is_shutdown():
# notify if low batt
#if self.robot.getCharger() < 10:
# print "battery low " + str(self.robot.getCharger()) + "%"
# get motor encoder values
left, right = self.robot.getMotors()
cmd_rate = cmd_rate-1
if cmd_rate ==0:
# send updated movement commands
#if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
# max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
#self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
cmd_rate = self.CMD_RATE
self.old_vel = self.cmd_vel
# prepare laser scan
scan.header.stamp = rospy.Time.now()
self.robot.requestScan()
scan.ranges = self.robot.getScanRanges()
# now update position information
dt = (scan.header.stamp - then).to_sec()
then = scan.header.stamp
d_left = (left - encoders[0])/1000.0
d_right = (right - encoders[1])/1000.0
encoders = [left, right]
#print d_left, d_right, encoders
dx = (d_left+d_right)/2
dth = (d_right-d_left)/(self.robot.base_width/1000.0)
x = cos(dth)*dx
y = -sin(dth)*dx
self.x += cos(self.th)*x - sin(self.th)*y
self.y += sin(self.th)*x + cos(self.th)*y
self.th += dth
#print self.x,self.y,self.th
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx/dt
odom.twist.twist.angular.z = dth/dt
# sensors
lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
# buttons
btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()
# publish everything
self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
quaternion.w), then, "base_footprint", "odom")
self.scanPub.publish(scan)
self.odomPub.publish(odom)
button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
if b == 1:
button.value = b
button.name = button_enum[idx]
self.buttonPub.publish(button)
for idx, b in enumerate((lsb, rsb, lfb, rfb)):
if b == 1:
sensor.value = b
sensor.name = sensor_enum[idx]
self.sensorPub.publish(sensor)
# wait, then do it again
r.sleep()
# shut down
self.robot.setBacklight(0)
self.robot.setLED("Off")
self.robot.setLDS("off")
self.robot.setTestMode("off")
def sign(self,a):
if a>=0:
return 1
else:
return-1
def cmdVelCb(self,req):
x = req.linear.x * 1000
th = req.angular.z * (self.robot.base_width/2)
k = max(abs(x-th),abs(x+th))
# sending commands higher than max speed will fail
if k > self.robot.max_speed:
x = x*self.robot.max_speed/k; th = th*self.robot.max_speed/k
self.cmd_vel = [int(x-th), int(x+th)]
class SMALdogROS:
def __init__(self, execute=False):
self.robot = SMALdog()
if execute:
self.conn = EthBridge()
else:
self.conn = None
self.x = 0
self.y = 0
self.joint_state_pub = rospy.Publisher('joint_states', JointState)
self.odom_broadcaster = TransformBroadcaster()
rospy.Subscriber("cmd_vel", Twist, self.cmdCb)
def run(self):
controller = MuybridgeGaitController(self.robot, self.robot.DEFAULT_STANCE)
old_pose = self.robot.getIK(self.robot.DEFAULT_STANCE)
old_pose["x"] = 0.0
old_pose["y"] = 0.0
old_pose["r"] = 0.0
draw = StabilityVisualization(self.robot)
r = rospy.Rate(50)
while not rospy.is_shutdown():
# get stance from controller TODO: this really should be a motion plan
new_stance = controller.next(self.x, 0, 0) # TODO: add y/r
t = new_stance[0]
#draw.draw(new_stance[1], controller)
# do IK
new_pose = self.robot.getIK(new_stance[1])
new_pose["x"] = controller.x
new_pose["y"] = controller.y
new_pose["r"] = controller.r
# interpolate
for pose in self.interpolate(old_pose, new_pose, int(t/0.02)):
# publish
msg = JointState()
msg.header.stamp = rospy.Time.now()
for name in self.robot.names:
msg.name.append(name)
msg.position.append(pose[name])
if pose[name] == float('nan'):
print "WARN", name, "is nan"
self.joint_state_pub.publish(msg)
# TF
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(pose["r"]/2)
quaternion.w = cos(pose["r"]/2)
self.odom_broadcaster.sendTransform(
(pose["x"], pose["y"], 0.095),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"body_link",
"world"
)
if self.conn:
packet = makeSyncWritePacket(convertToAX12(pose, self.robot))
self.conn.send(self.conn.makePacket(254, ax12.AX_SYNC_WRITE, packet))
r.sleep()
old_pose = new_pose
def interpolate(self, start_pose, end_pose, iterations):
diffs = dict()
for name in start_pose.keys():
diffs[name] = (end_pose[name] - start_pose[name])/iterations
for i in range(iterations):
pose = dict()
for name in start_pose.keys():
pose[name] = start_pose[name] + (diffs[name]*i)
yield pose
def cmdCb(self, msg):
# TODO: add y/r
if msg.linear.x > 0.075:
self.x = 0.075
else:
self.x = msg.linear.x
class ThymioDriver():
# ======== class initialization ========
def __init__(self):
rospy.init_node('thymio')
# initialize parameters
self.x = 0
self.y = 0
self.th = 0
self.then = rospy.Time.now()
self.odom = Odometry(header=rospy.Header(frame_id='odom'),child_frame_id='base_link')
# load script on the Thymio
rospy.wait_for_service('aseba/load_script')
load_script = rospy.ServiceProxy('aseba/load_script',LoadScripts)
script_filename = roslib.packages.get_pkg_dir('thymio_driver') + '/aseba/thymio_ros.aesl'
load_script(script_filename)
# subscribe to topics
self.aseba_pub = rospy.Publisher('aseba/events/set_speed', AsebaEvent,queue_size=1)
self.odom_pub = rospy.Publisher('odom',Odometry,queue_size=1)
self.odom_broadcaster = TransformBroadcaster()
rospy.Subscriber('aseba/events/odometry', AsebaEvent, self.on_aseba_odometry_event)
rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)
self.buttons=Joy()
self.buttons_pub=rospy.Publisher('buttons',Joy,queue_size=1)
rospy.Subscriber("aseba/events/buttons",AsebaEvent,self.on_aseba_buttons_event)
for button in BUTTONS:
rospy.Subscriber('aseba/events/button_'+button,AsebaEvent,self.on_aseba_button_event(button))
self.proximity_sensors=[{
'publisher':rospy.Publisher('proximity/'+name,Range,queue_size=1),
'msg':Range(header=rospy.Header(frame_id='proximity_'+name+"_link"),radiation_type=Range.INFRARED,field_of_view=0.3,min_range=0.0215,max_range=0.14)
} for name in PROXIMITY_NAMES]
self.proximityToLaserPublisher=rospy.Publisher('proximity/laser',LaserScan,queue_size=1)
self.proximityToLaser=LaserScan(header=rospy.Header(frame_id="base_link"),angle_min=-0.64,angle_max=0.64,angle_increment=0.32,time_increment=0,scan_time=0,range_min=0.0215+0.08,range_max=0.14+0.08)
rospy.Subscriber('aseba/events/proximity',AsebaEvent,self.on_aseba_proximity_event)
self.ground_sensors=[{
'publisher':rospy.Publisher('ground/'+name,Range,queue_size=1),
'msg':Range(header=rospy.Header(frame_id='ground_'+name+"_link"),radiation_type=Range.INFRARED,field_of_view=0.3,min_range=0.008,max_range=0.008)
} for name in GROUND_NAMES]
rospy.Subscriber('aseba/events/ground',AsebaEvent,self.on_aseba_ground_event)
rospy.set_param("~ground_threshold",200)
self.imu=Imu(header=rospy.Header(frame_id='base_link'))
# no orientation or angular velocity information
self.imu.orientation_covariance[0]=-1
self.imu.angular_velocity_covariance[0]=-1
# just an accelerometer
self.imu.linear_acceleration_covariance[0]=0.07
self.imu.linear_acceleration_covariance[4]=0.07
self.imu.linear_acceleration_covariance[8]=0.07
self.imu_publisher=rospy.Publisher('imu',Imu,queue_size=1)
rospy.Subscriber('aseba/events/accelerometer',AsebaEvent,self.on_aseba_accelerometer_event)
self.tap_publisher=rospy.Publisher('tap',Empty,queue_size=1)
rospy.Subscriber('aseba/events/tap',AsebaEvent,self.on_aseba_tap_event)
self.temperature=Temperature(header=rospy.Header(frame_id='base_link'))
self.temperature.variance=0.01
self.temperature_publisher=rospy.Publisher('temperature',Temperature,queue_size=1)
rospy.Subscriber('aseba/events/temperature',AsebaEvent,self.on_aseba_temperature_event)
self.sound_publisher=rospy.Publisher('sound',Float32,queue_size=1)
self.sound_threshold_publisher = rospy.Publisher('aseba/events/set_sound_threshold', AsebaEvent,queue_size=1)
rospy.Subscriber('aseba/events/sound',AsebaEvent,self.on_aseba_sound_event)
rospy.Subscriber('sound_threshold',Float32,self.on_sound_threshold)
self.remote_publisher=rospy.Publisher('remote',Int8,queue_size=1)
rospy.Subscriber('aseba/events/remote',AsebaEvent,self.on_aseba_remote_event)
rospy.Subscriber('comm/transmit',Int16,self.on_sound_threshold)
self.comm_publisher=rospy.Publisher('comm/receive',Int16,queue_size=1)
self.aseba_set_comm_publisher = rospy.Publisher('aseba/events/set_comm', AsebaEvent,queue_size=1)
rospy.Subscriber('aseba/events/comm',AsebaEvent,self.on_aseba_comm_event)
#actuators
for name in BODY_LEDS:
rospy.Subscriber('led/body/'+name,ColorRGBA,self.on_body_led(name))
rospy.Subscriber('led',Led,self.on_led)
self.aseba_led_publisher=rospy.Publisher('aseba/events/set_led', AsebaEvent,queue_size=6)
rospy.Subscriber('led/off',Empty,self.on_led_off)
rospy.Subscriber('led/gesture',LedGesture,self.on_led_gesture)
self.aseba_led_gesture_publisher=rospy.Publisher('aseba/events/set_led_gesture', AsebaEvent,queue_size=6)
rospy.Subscriber('led/gesture/circle',Float32,self.on_led_gesture_circle)
rospy.Subscriber('led/gesture/off',Empty,self.on_led_gesture_off)
rospy.Subscriber('led/gesture/blink',Float32,self.on_led_gesture_blink)
rospy.Subscriber('led/gesture/kit',Float32,self.on_led_gesture_kit)
rospy.Subscriber('led/gesture/alive',Empty,self.on_led_gesture_alive)
rospy.Subscriber('sound/play',Sound,self.on_sound_play)
self.aseba_led_gesture_publisher=rospy.Publisher('aseba/events/set_led_gesture', AsebaEvent,queue_size=6)
rospy.Subscriber('sound/play/system',SystemSound,self.on_system_sound_play)
self.aseba_play_sound_publisher=rospy.Publisher('aseba/events/play_sound', AsebaEvent,queue_size=1)
self.aseba_play_system_sound_publisher=rospy.Publisher('aseba/events/play_system_sound', AsebaEvent,queue_size=1)
rospy.Subscriber('alarm',Bool,self.on_alarm)
self.alarm_timer=None
rospy.Subscriber('shutdown',Empty,self.on_shutdown_msg)
self.aseba_shutdown_publisher=rospy.Publisher('aseba/events/shutdown', AsebaEvent,queue_size=1)
#tell ros that we are ready
rospy.Service('thymio_is_ready',std_srvs.srv.Empty, self.ready)
def on_shutdown_msg(self,msg):
self.aseba_shutdown_publisher.publish(AsebaEvent(rospy.get_rostime(),0,[]))
def ready(self,req):
return std_srvs.srv.Empty()
def play_system_sound(self,sound):
self.aseba_play_system_sound_publisher.publish(AsebaEvent(rospy.get_rostime(),0,[sound]))
def alarm_cb(self,evt):
self.play_system_sound(2)
def on_alarm(self,msg):
if msg.data and not self.alarm_timer:
self.alarm_timer=rospy.Timer(rospy.Duration(3),self.alarm_cb)
if msg.data==False and self.alarm_timer:
self.alarm_timer.shutdown()
self.alarm_timer=None
def on_sound_play(self,msg):
freq=max(1,int(msg.frequency))
duration=max(1,int(msg.duration.to_sec()*60))
self.aseba_play_sound_publisher.publish(AsebaEvent(rospy.get_rostime(),0,[freq,duration]))
def on_system_sound_play(self,msg):
self.play_system_sound(msg.sound)
def set_led_gesture(self,gesture,leds,wave,period,length,mirror,mask):
period=max(-32678,min(32678,int(period*1000)))
data=[gesture,leds,wave,period,length,mirror]+mask[:8]
data+=[1]*(14-len(data))
self.aseba_led_gesture_publisher.publish(AsebaEvent(rospy.get_rostime(),0,data))
def on_led_gesture(self,msg):
self.set_led_gesture(msg.gesture,msg.leds,msg.wave,msg.period,msg.length,msg.mirror,msg.mask)
def on_led_gesture_off(self,msg):
self.set_led_gesture(LedGesture.OFF,0,0,0,0,0,[])
def on_led_gesture_circle(self,msg):
self.set_led_gesture(LedGesture.WAVE,LedGesture.CIRCLE,LedGesture.HARMONIC,msg.data,8,0,[])
def on_led_gesture_blink(self,msg):
self.set_led_gesture(LedGesture.WAVE,LedGesture.CIRCLE,LedGesture.HARMONIC,msg.data,1,0,[])
def on_led_gesture_kit(self,msg):
self.set_led_gesture(LedGesture.WAVE,LedGesture.PROXIMITY,LedGesture.HARMONIC,msg.data,12,11,[1,1,1,1,1,1,0,0])
def on_led_gesture_alive(self,msg):
self.set_led_gesture(LedGesture.WAVE,LedGesture.CIRCLE,LedGesture.RECT,3.0,24,0,[])
def on_led_off(self,msg):
for i in LED_NUMBER.keys():
print 'off ',i
self.aseba_led_publisher.publish(AsebaEvent(rospy.get_rostime(),0,[i]+8*[0]))
# sleep to avoid that aseba or ros do not process all messages.
# could be improved by having 6 separate aseba topics where to send messages
rospy.sleep(0.005)
def on_led(self,msg):
i=msg.id
num=LED_NUMBER.get(i,0)
if num<=len(msg.values):
data=[i]+[int(32*v) for v in msg.values[:8]]
data+=[0]*(9-len(data))
self.aseba_led_publisher.publish(AsebaEvent(rospy.get_rostime(),0,data))
def on_body_led(self,name):
publisher=rospy.Publisher('aseba/events/set_led_'+name,AsebaEvent,queue_size=1)
def callback(msg):
r=int(msg.r*32)
g=int(msg.g*32)
b=int(msg.b*32)
aseba_msg=AsebaEvent(rospy.get_rostime(),0,[r,g,b])
publisher.publish(aseba_msg)
return callback
def on_set_comm(self,msg):
self.aseba_set_comm_publisher.publish(AsebaEvent(rospy.get_rostime(),0,[enabled,payload]))
def on_aseba_comm_event(self,msg):
self.comm_publisher.publish(Int16(msg.data[0]))
def on_aseba_remote_event(self,msg):
self.remote_publisher.publish(Int8(msg.data[1]))
def on_sound_threshold(self,msg):
value=msg*255
if value<0:
value=1
if value>255:
value=0
self.sound_threshold_publisher.publish(AsebaEvent(rospy.get_rostime(),0,[value]))
def on_aseba_sound_event(self,msg):
self.sound_publisher.publish(Float32(msg.data[0]/255.0))
def on_aseba_tap_event(self,msg):
self.tap_publisher.publish(Empty())
def on_aseba_temperature_event(self,msg):
self.temperature.temperature=msg.data[0]/10.0
self.temperature_publisher.publish(self.temperature)
#TODO check how it's implemented in the firmware.
def on_aseba_accelerometer_event(self,msg):
self.imu.linear_acceleration.x=msg.data[1]/23.0*9.81
self.imu.linear_acceleration.y=-msg.data[0]/23.0*9.81
self.imu.linear_acceleration.z=msg.data[2]/23.0*9.81
self.imu.header.stamp=rospy.Time.now()
self.imu_publisher.publish(self.imu)
def on_aseba_ground_event(self,msg):
data=msg.data
ir_threshold=rospy.get_param("~ground_threshold",200)
for sensor,value in zip(self.ground_sensors,data):
sensor['msg'].range=float('inf') if (value<ir_threshold) else -float('inf')
sensor['msg'].header.stamp=rospy.Time.now()
sensor['publisher'].publish(sensor['msg'])
# basics logarithmic fit
@staticmethod
def proximity2range(raw):
if raw>4000:
return -float('inf')
if raw<800:
return float('inf')
return -0.0736*log(raw)+0.632
def on_aseba_proximity_event(self,msg):
data=msg.data
values=[self.proximity2range(d) for d in data]
for sensor,value in zip(self.proximity_sensors,values):
sensor['msg'].range=value
sensor['msg'].header.stamp=rospy.Time.now()
sensor['publisher'].publish(sensor['msg'])
self.proximityToLaser.ranges=[]
self.proximityToLaser.intensities=[]
self.proximityToLaser.header.stamp=rospy.Time.now()
for dist,raw in zip(values,data)[4::-1]:
if dist>0.14:
dist=0.14
if dist<0.0215:
dist=0.0215
self.proximityToLaser.ranges.append(dist+0.08)
self.proximityToLaser.intensities.append(raw)
self.proximityToLaserPublisher.publish(self.proximityToLaser)
def on_aseba_button_event(self,button):
publisher=rospy.Publisher('buttons/'+button,Bool,queue_size=1)
def callback(msg):
bool_msg=Bool(msg.data[0])
publisher.publish(bool_msg)
return callback
# ======== we send the speed to the aseba running on the robot ========
def set_speed(self,values):
self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(),0,values))
# ======== stop the robot safely ========
def shutdown(self):
self.set_speed([0,0])
def on_aseba_buttons_event(self,data):
self.buttons.header.stamp=rospy.Time.now()
self.buttons.buttons=data.data
self.buttons_pub.publish(self.buttons)
# ======== processing odometry events received from the robot ========
def on_aseba_odometry_event(self,data):
now = data.stamp
dt = (now-self.then).to_sec()
self.then = now
dsl = (data.data[0]*dt)/SPEED_COEF # left wheel delta in mm
dsr = (data.data[1]*dt)/SPEED_COEF # right wheel delta in mm
ds = ((dsl+dsr)/2.0)/1000.0 # robot traveled distance in meters
dth = (dsr-dsl)/BASE_WIDTH # turn angle
self.x += ds*cos(self.th+dth/2.0)
self.y += ds*sin(self.th+dth/2.0)
self.th+= dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
self.odom.header.stamp = rospy.Time.now() # OR TO TAKE ONE FROM THE EVENT?
self.odom.pose.pose.position.x = self.x
self.odom.pose.pose.position.y = self.y
self.odom.pose.pose.position.z = 0
self.odom.pose.pose.orientation = quaternion
if(dt>0):
self.odom.twist.twist.linear.x = ds/dt
self.odom.twist.twist.angular.z = dth/dt
# publish odometry
self.odom_broadcaster.sendTransform((self.x,self.y,0),(quaternion.x,quaternion.y,quaternion.z,quaternion.w),self.then,"base_link","odom")
self.odom_pub.publish(self.odom)
# ======== processing events received from the robot ========
def on_cmd_vel(self,data):
x = data.linear.x * 1000.0 # from meters to millimeters
x = x * SPEED_COEF # to thymio units
th = data.angular.z * (BASE_WIDTH/2) # in mm
th = th * SPEED_COEF # in thymio units
k = max(abs(x-th),abs(x+th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
self.set_speed([int(x-th),int(x+th)])
# ======== ======== ======== ======== ======== ======== ========
def control_loop(self):
rospy.on_shutdown(self.shutdown) # on shutdown hook
while not rospy.is_shutdown():
rospy.spin()
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
#Wheel radius : 0.0325
# wheel circum = 2* 3.14 * 0.0325 = 0.2041
# One rotation encoder ticks : 8 ticks
# For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 39)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.125)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.yaw = 0.01
self.pitch = 0.01
self.roll = 0.01
self.then = rospy.Time.now()
self.quaternion_1 = Quaternion()
# subscriptions
rospy.Subscriber("left_ticks", Int32, self.lwheelCallback)
rospy.Subscriber("right_ticks", Int32, self.rwheelCallback)
#rospy.Subscriber("imu_data", Vector3, self.imu_value_update)
self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = ( d_left + d_right ) / 2
# this approximation works (in radians) for small angles
th = ( d_right - d_left ) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos( th ) * d
y = -sin( th ) * d
# calculate the final position of the robot
self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
if( th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin( self.th / 2 )
quaternion.w = cos( self.th / 2 )
'''
try:
quaternion.z = self.quaternion_1[2]
quaternion.w = self.quaternion_1[3]
except:
quaternion.z = sin( self.th / 2 )
quaternion.w = cos( self.th / 2 )
pass
'''
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
def imu_value_update(self, imu_data):
orient = Vector3()
orient = imu_data
self.yaw = orient.x
self.pitch = orient.y
self.roll = orient.z
try:
self.quaternion_1 = tf.transformations.quaternion_from_euler(self.yaw, self.pitch, self.roll)
#print self.quaternion_1[0]
#print self.quaternion_1[1]
#print self.quaternion_1[2]
#print self.quaternion_1[3]
except:
rospy.logwarn("Unable to get quaternion values")
pass
#############################################################################
def lwheelCallback(self, msg):
#############################################################################
enc = msg.data
if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap):
self.lmult = self.lmult + 1
if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap):
self.lmult = self.lmult - 1
self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min))
self.prev_lencoder = enc
#############################################################################
def rwheelCallback(self, msg):
#############################################################################
enc = msg.data
if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap):
self.rmult = self.rmult + 1
if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap):
self.rmult = self.rmult - 1
self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min))
self.prev_rencoder = enc
class odom():
def __init__(self):
self.sub = rospy.Subscriber('/encoders', encoders, self.my_callback)
self.pub = rospy.Publisher('/odom', Odometry, queue_size=1)
self.odo = Odometry()
self.read = encoders()
self.Fistleftenco_tick = 0
self.Firstrightenco_tick = 0
rospy.sleep(0.5)
self.R = 0.1
self.L = 0.3
self.N = 360
self.x = 0
self.y = 0
self.odom_broadcaster = TransformBroadcaster()
self.theta = 0.0
self.last_time = rospy.Time.now()
def my_callback(self, msg):
self.read = msg
self.Lastleftenco_tick = self.read.encoderTicks[0]
self.Lastrightenco_tick = self.read.encoderTicks[1]
def readings(self):
while not rospy.is_shutdown():
present_time = rospy.Time.now()
Dr = 2 * pi * self.R * (self.Fistleftenco_tick -
self.Lastleftenco_tick) / self.N
Dl = 2 * pi * self.R * (self.Firstrightenco_tick -
self.Lastrightenco_tick) / self.N
self.Firstrightenco_tick = self.Lastrightenco_tick
self.Fistleftenco_tick = self.Lastleftenco_tick
v = (Dr + Dl) / 2
w = (Dr - Dl) / self.L
self.x += v * cos(self.theta)
self.y += v * sin(self.theta)
self.theta += w
#get the quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.theta)
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0.), odom_quat, present_time, "link_chassis",
"odom")
self.odo.header.stamp = present_time
self.odo.header.frame_id = "odom"
self.odo.pose.pose = Pose(Point(self.x, self.y, 0.),
Quaternion(*odom_quat))
self.odo.child_frame_id = "link_chassis"
self.odo.twist.twist = Twist(Vector3(v, 0, 0), Vector3(0, 0, w))
self.pub.publish(self.odo)
self.last_time = present_time
rospy.sleep(0.1)
class NeatoDriver:
def __init__(self):
self.neato = xv11()
self.sensorPub = rospy.Publisher('neatoSensors', NeatoSensors) # see NOTE above
self.rangePub = rospy.Publisher('laserRangeData',LaserRangeData)
self.fields = self.neato.state.keys() # gets all of the sensors fields from the Neato
self.neato.update = self.sense
# for gmapping
self.odomBroadcaster = TransformBroadcaster()
# for odometry testing
self.prevL = 0
self.prevR = 0
self.x = 0
self.y = 0
self.theta = 0
self.odom = NeatoOdom()
self.odomPub = rospy.Publisher('neatoOdom', NeatoOdom)
def start(self):
self.neato.start()
def stop(self, req):
self.neato.stop()
return StopResponse(True)
def tank(self, req):
self.neato.tank(req.left,req.right)
return TankResponse(True)
def playSound(self, req):
""" plays a sound from the Neato's database.
num goes from 0 to 20 inclusive
"""
self.neato.playSound(req.num)
return PlaySoundResponse(True)
def sense(self):
""" collects sensor data from the Neato and publishes
it to the neatoSensors topic
"""
# receive and publish sensor data
self.fields = self.neato.state.keys() # update sensor fields
sensor_data = NeatoSensors() # see NOTE above
for field in self.fields:
try:
sensor_data.__setattr__(field, self.neato.state[field])
except:
pass
self.sensorPub.publish(sensor_data)
# receive and publish laser range data
range_data = LaserRangeData()
range_data.__setattr__("range_data", self.neato.range_data)
self.rangePub.publish(range_data)
# odomemtry in testing
self.odomUpdate()
# transform position into tf frame
quaternionOdom = Quaternion()
quaternionOdom.x = 0.0
quaternionOdom.y = 0.0
quaternionOdom.z = sin(self.theta/2)
quaternionOdom.w = -cos(self.theta/2)
quaternionLL = Quaternion()
quaternionLL.x = 0.0
quaternionLL.y = 0.0
quaternionLL.z = 0.0
quaternionLL.w = 1.0
# base_link -> base_laser transformation
self.odomBroadcaster.sendTransform(
(0.0, -0.1, 0.0),
(quaternionLL.x, quaternionLL.y, quaternionLL.z, quaternionLL.w),
rospy.Time.now(),
"/base_laser",
"/base_link")
# odom -> base_link transformation
self.odomBroadcaster.sendTransform(
(self.x/1000, self.y/1000, 0),
(quaternionOdom.x, quaternionOdom.y, quaternionOdom.z, quaternionOdom.w),
rospy.Time.now(),
"/base_link",
"/odom")
def odomUpdate(self):
""" updates the odometry of the robot by using the measured
wheel distances and wheelbase
"""
WHEEL_BASE = 237.5 # measured in mm
odom = NeatoOdom()
LWheelMM = self.neato.state["LeftWheel_PositionInMM"]
RWheelMM = self.neato.state["RightWheel_PositionInMM"]
# calculate difference in position from last time
Ldist = LWheelMM - self.prevL
Rdist = RWheelMM - self.prevR
# set new measurements to be the old ones
self.prevL = LWheelMM
self.prevR = RWheelMM
dist = (Ldist + Rdist)/2.0
self.theta += (Ldist - Rdist)/WHEEL_BASE
self.x += dist * sin(self.theta)
self.y += dist * cos(self.theta)
##print self.x, self.y
odom.__setattr__("theta", self.theta)
odom.__setattr__('x', self.x)
odom.__setattr__('y', self.y)
self.odomPub.publish(odom)
class RoverpiOdometry:
def __init__(self):
self.x = 0.0
self.y = 0.0
self.th = 0.0
self.vx = 0.0
self.vy = 0.0
self.vth = 0.0
self.vx_max = 1.0
self.vth_max = 1.0
self.left_wheel_sub = rospy.Subscriber("lwheel_rpm", Int32,
self.left_callback)
self.right_wheel_sub = rospy.Subscriber("rwheel_rpm", Int32,
self.right_callback)
self.initial_pose_sub = rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.on_initial_pose)
self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist,
self.cmd_vel_callback)
self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=100)
self.odom_broadcaster = TransformBroadcaster()
self.current_time = rospy.Time.now()
self.previous_time = rospy.Time.now()
self.last_cmd_time = None
def on_initial_pose(self, msg):
q = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.x, msg.pose.pose.orientation.w
]
roll, pitch, yaw = euler_from_quaternion(q)
self.x = msg.pose.pose.position.x
self.y = msg.pose.pose.position.y
self.th = yaw
def left_callback(self, msg):
print("Left Wheel RPM = %d", msg.data)
def right_callback(self, msg):
print("Right Wheel RPM = %d", msg.data)
def cmd_vel_callback(self, msg):
self.vx = max(min(msg.linear.x, self.vx_max), -self.vx_max)
self.vth = max(min(msg.angular.z, self.vth_max), -self.vth_max)
self.last_cmd_time = rospy.Time.now()
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.current_time = rospy.Time.now()
if self.last_cmd_time == None or (
self.current_time - self.last_cmd_time).to_sec() > 1.0:
rospy.loginfo("Did not get command for 1 second, stopping")
else:
# compute odometry in a typical way given the velocities of the robot
dt = (self.current_time - self.previous_time).to_sec()
delta_x = (self.vx * cos(self.th) -
self.vy * sin(self.th)) * dt
delta_y = (self.vx * sin(self.th) +
self.vy * cos(self.th)) * dt
delta_th = self.vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
# create quaternion from yaw
odom_quat = quaternion_from_euler(0, 0, self.th)
# publish the transform over tf
self.odom_broadcaster.sendTransform((self.x, self.y, 0), odom_quat,
self.current_time, "base_link",
"odom")
# construct odometry message
odom = Odometry()
odom.header.stamp = self.current_time
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation.x = odom_quat[0]
odom.pose.pose.orientation.y = odom_quat[1]
odom.pose.pose.orientation.z = odom_quat[2]
odom.pose.pose.orientation.w = odom_quat[3]
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = self.vy
odom.twist.twist.angular.z = self.vth
# publish the message
self.odom_pub.publish(odom)
self.previous_time = self.current_time
rate.sleep()
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("process")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param("~rate", 50.0) # the rate at which to publish the transform
self.base_frame_id = rospy.get_param("~base_frame_id", "base_link") # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param("~odom_frame_id", "odom") # the name of the odometry reference frame
self.laser_frame_id = rospy.get_param("~laser_frame_id", "laser_link")
self.map_frame_id = rospy.get_param("~map_frame_id", "map")
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.th_pre = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
self.qw = 0
self.qx = 0
self.qy = 0
self.qz = 0
self.laser = LaserScan()
# subscriptions
rospy.Subscriber("qx", Float32, self.qx_update)
rospy.Subscriber("qy", Float32, self.qy_update)
rospy.Subscriber("qz", Float32, self.qz_update)
rospy.Subscriber("qw", Float32, self.qw_update)
rospy.Subscriber("th", Float32, self.th_update)
rospy.Subscriber("scan", LaserScan, self.laser_update)
rospy.Subscriber("px", Float32, self.x_update)
rospy.Subscriber("py", Float32, self.y_update)
rospy.Subscriber("dx", Float32, self.dx_update)
# rospy.Subscriber('dr',Float32,self.dr_update)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.laserPub = rospy.Publisher("laser_scan", LaserScan, queue_size=20)
self.odomBroadcaster = TransformBroadcaster()
self.laserBroadcaster = TransformBroadcaster()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# this approximation works (in radians) for small angles
th = self.th - self.th_pre
self.dr = th / elapsed
# publish the odom information
quaternion = Quaternion()
quaternion.x = self.qx
quaternion.y = self.qy
quaternion.z = self.qz
quaternion.w = self.qw
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(0, 0, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id,
)
self.laserBroadcaster.sendTransform(
(0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
laser = LaserScan()
laser.header.stamp = now
laser.header.frame_id = self.laser_frame_id
laser.angle_min = self.laser.angle_min
laser.angle_max = self.laser.angle_max
laser.angle_increment = self.laser.angle_increment
laser.time_increment = self.laser.time_increment
laser.scan_time = self.laser.scan_time
laser.range_min = self.laser.range_min
laser.range_max = self.laser.range_max
laser.ranges = self.laser.ranges
laser.intensities = self.laser.intensities
self.laserPub.publish(laser)
def qx_update(self, msg):
self.qx = msg.data
def qy_update(self, msg):
self.qy = msg.data
def qz_update(self, msg):
self.qz = msg.data
def qw_update(self, msg):
self.qw = msg.data
def th_update(self, msg):
self.th_pre = self.th
self.th = msg.data
def laser_update(self, msg):
self.laser = msg
def x_update(self, msg):
self.x = msg.data
def y_update(self, msg):
self.y = msg.data
def dx_update(self, msg):
self.dx = msg.data
class ARbotController:
def __init__(self, arduino):
self.arduino = arduino
self.stopped = False
self.cmd = 1
self.forwardSpeed = 0
self.angularSpeed = 0
# Subscribe to cmd_vel
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
s = rospy.Service("pick_cmd_vel", PickCmdVel, self.pickCmdVel)
# Subscribe to arbot_play_led and arbot_advance_led
rospy.Subscriber("arbot_play_led", Bool, self.arbotPlayLedCallback)
rospy.Subscriber("arbot_advance_led", Bool, self.arbotAdvanceLedCallback)
# Setup the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
# Setup blob publisher
# self.blob_publisher = rospy.Publisher('blobs', Blobs, queue_size=10)
rospy.loginfo("Started ARbot controller.")
def poll(self):
(x, y, theta) = self.arduino.arbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
now,
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.angularSpeed
self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)
self.odomPub.publish(odom)
# blobs = self.arduino.get_blobs()
# blobs.header.stamp = rospy.Time.now()
# self.blob_publisher.publish(blobs)
def stop(self):
self.stopped = True
self.forwardSpeed = 0
self.angularSpeed = 0
self.arduino.arbot_set_velocity(0, 0)
def cmdVelCallback(self, cmd_vel):
if self.cmd == 2:
return
self.forwardSpeed = cmd_vel.linear.x # m/s
self.angularSpeed = cmd_vel.angular.z # rad/s
def cmdVel2Callback(self, cmd_vel2):
if self.cmd == 1:
return
self.forwardSpeed = cmd_vel2.linear.x
self.angularSpeed = cmd_vel2.angular.z
def pickCmdVel(self, req):
self.cmd = req.cmd_vel
#print "got cmd_vel: {0}".format(req.cmd_vel)
return PickCmdVelResponse(req.cmd_vel)
def arbotPlayLedCallback(self, msg):
self.arduino.arbot_set_play_led(mgs.data)
def arbotAdvanceLedCallback(self, msg):
self.arduino.arbot_set_advance_led(msg.data)
def wheelsOdomSendCmd():
global linearVelocityCmd
global angularVelocityCmd
global lastCmdVelTime
pubOdom=rospy.Publisher('odom', Odometry, queue_size=2)
pubOdomBroadcaster = TransformBroadcaster()
rospy.init_node('wheels', anonymous = True)
rate =rospy.Rate(200)
lastCmdVelTime=rospy.Time.now()
wheelBase=rospy.get_param("~distance_between_wheels")
ticksPerRev=rospy.get_param("~wheel_encoder_ticks_per_revolution")
wheelCircumference=rospy.get_param("~drive_wheel_circumference")
baseFrame=rospy.get_param("~base_frame")
port=(rospy.get_param("~port_wheels_motordriver"))
address=rospy.get_param("~address_wheels_motordriver",0X80)
sendToMDRate=rospy.get_param("~rate_to_send_cmd_vel_to_motor_driver",30.0)
nseconds=1000000000/sendToMDRate
timeBetweenMDCMD=rospy.Duration(0, nseconds)
MDtime=rospy.Time.now()
metersToTicks=ticksPerRev/wheelCircumference
ticksToMeters=wheelCircumference/ticksPerRev
#establish connection to Motor driver
start_connect = time.time()
TIMEOUT=False
while TIMEOUT==False:
try:
roboclaw.Open(port,115200)#/dev/roboclaw_wheels
break
except Exception as e:
if time.time()-start_connect > 60:
rospy.logfatal("failed to connect to wheels motor driver")
TIMEOUT=True
break
print "Failed to connect to wheels motor driver, retrying...\n"
time.sleep(5)
pass
version = roboclaw.ReadVersion(address)
if version[0]==False:
rospy.logfatal("get version of wheel motor driver failed")
else:
repr(version[1])
rospy.loginfo("connected to motor driver")
roboclaw.ResetEncoders(address)#set encoder values to 0
roboclaw.SpeedM1M2(address,0,0)#set motor speed to 0
lastLeftDistance=0.0
lastRightDistance=0.0
lastLeftSpeed=0.0
lastRightSpeed=0.0
yaw=0.0
positionX=0
positionY=0
#setup subscriber
rospy.Subscriber("/cmd_vel", Twist, cmdVelCallback)
seconds=rospy.Time()
while not rospy.is_shutdown():
now = rospy.Time.now()
try:
lt=roboclaw.ReadEncM1(address)
rt=roboclaw.ReadEncM2(address)
ls=roboclaw.ReadSpeedM1(address)
rs=roboclaw.ReadSpeedM1(address)
except:
version = roboclaw.ReadVersion(address)
while version[0]==False:
try:
roboclaw.Open(port,115200)#/dev/roboclaw_wheels
except rospy.ROSInterruptException:
rospy.logwarn("failed to connect to wheels motor driver again afer it disconeced")
pass
rospy.sleep(0.05)
leftDistance=lt[1]*ticksToMeters
rightDistance=rt[1]*ticksToMeters
leftSpeed=ls[1]*ticksToMeters
rightSpeed=rs[1]*ticksToMeters
#rospy.loginfo(" Left distance %d right distance %d", lt[1], rt[1])
#rospy.loginfo("left %d right %d", leftDistance, rightDistance)
dLeftDistance=leftDistance-lastLeftDistance
dRightDistance=rightDistance-lastRightDistance
rotation=(dRightDistance-dLeftDistance)/wheelBase
dLeftSpeed=leftSpeed-lastLeftSpeed
dRightSpeed=rightSpeed-lastRightSpeed
rotationSpeed=(dRightSpeed-dLeftSpeed)/wheelBase
averageDistance=(dRightDistance+dLeftDistance)/2.0
averageVelocity=(dRightSpeed+dLeftSpeed)/2.0
lastLeftDistance=leftDistance
lastRightDistance=rightDistance
lastLeftSpeed=leftSpeed
lastRightDistance=rightDistance
if (averageDistance !=0) :
dx=cos(rotation)*averageDistance
dy=-sin(rotation)*averageDistance
positionX += cos(yaw)*dx-sin(yaw)*dy
positionY +=sin(yaw)*dx+cos(yaw)*dy
if(rotation != 0):
yaw+=rotation
quaternion=Quaternion()
quaternion.x=0.0
quaternion.y=0.0
quaternion.z=sin(yaw/2.0)
quaternion.w=cos(yaw/2.0)
# Create the odometry transform frame broadcaster.
pubOdomBroadcaster.sendTransform(
(positionX, positionY, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
baseFrame,
"odom"
)
#rospy.loginfo(" linear %f angular %f", linearVelocityCmd, angularVelocityCmd)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = baseFrame
odom.header.stamp = now
odom.pose.pose.position.x = positionX
odom.pose.pose.position.y = positionY
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = averageVelocity
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = rotationSpeed
pubOdom.publish(odom)
#get desired wheel velocities
if (rospy.Time.now()-MDtime)>=timeBetweenMDCMD:
MDtime=rospy.Time.now()
x = linearVelocityCmd # m/s
th = angularVelocityCmd # rad/s
#rospy.loginfo ("Linear cmd %f , angular cmd %f", x, th)
if x == 0:
# Turn in place
right = th * wheelBase / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * wheelBase/ 2.0
right = x + th * wheelBase / 2.0
try:
left=int(left*metersToTicks)
right=int(right*metersToTicks)
except:
left = 0
right = 0
try:
roboclaw.SpeedM1M2(address,left,right)
except:
version = roboclaw.ReadVersion(address)
while version[0]==False:
try:
roboclaw.Open(port,115200)#/dev/roboclaw_wheels
except:
rospy.logwarn("failed to connect to wheels motor driver again afer it disconeced")
pass
rospy.sleep(0.05)
rate.sleep()
# turn off motors if ros is shutdown
roboclaw.SpeedM1M2(address,0,0)
class WheelchairTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("wheelchair_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param(
'~rate', 20.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param(
'ticks_meter',
3776.52)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param(
'~base_width', 0.615)) # The wheel base width in meters
self.base_frame_id = rospy.get_param(
'~base_frame_id',
'base_footprint') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param(
'~odom_frame_id',
'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param(
'wheel_low_wrap',
(self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
self.encoder_high_wrap = rospy.get_param(
'wheel_high_wrap',
(self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # orientation in radians
self.prev_th = 0 # previous orientation in radians
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
self.first_instance = 1
self.th_offset = 0
# subscriptions
rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
rospy.Subscriber("kalmanOutput", Float32, self.thetaCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = (d_left + d_right) / 2
# this approximation works (in radians) for small angles
#th = ( d_right - d_left ) / self.base_width
th = self.th - self.prev_th
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos(self.th) * d
y = sin(self.th) * d
# calculate the final position of the robot
self.x = self.x + x
self.y = self.y + y
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
#############################################################################
def lwheelCallback(self, msg):
#############################################################################
enc = msg.data
if (enc < self.encoder_low_wrap
and self.prev_lencoder > self.encoder_high_wrap):
self.lmult = self.lmult + 1
if (enc > self.encoder_high_wrap
and self.prev_lencoder < self.encoder_low_wrap):
self.lmult = self.lmult - 1
self.left = 1.0 * (enc + self.lmult *
(self.encoder_max - self.encoder_min))
#rospy.loginfo(msg.data)
self.prev_lencoder = enc
#############################################################################
def rwheelCallback(self, msg):
#############################################################################
enc = msg.data
if (enc < self.encoder_low_wrap
and self.prev_rencoder > self.encoder_high_wrap):
self.rmult = self.rmult + 1
if (enc > self.encoder_high_wrap
and self.prev_rencoder < self.encoder_low_wrap):
self.rmult = self.rmult - 1
self.right = 1.0 * (enc + self.rmult *
(self.encoder_max - self.encoder_min))
self.prev_rencoder = enc
#############################################################################
def thetaCallback(self, msg):
#############################################################################
if (self.first_instance < 50):
self.th_offset = msg.data
self.first_instance += 1
if (self.first_instance > 100):
self.first_instance = 100
self.prev_th = self.th
#self.th = msg.data
self.th = (msg.data - self.th_offset) * (3.1415 / 180.0)
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.port = rospy.get_param('~port', "/dev/ttyUSB0")
rospy.loginfo("Using port: %s"%(self.port))
self.robot = xv11(self.port)
rospy.Subscriber("pi_cmd",String,self.pi_command)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('scan', LaserScan, queue_size=10)
self.odomPub = rospy.Publisher('odom',Odometry, queue_size=10)
self.bumpPub = rospy.Publisher('bump',Bump, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_to_send = None
self.cmd_vel = [0,0]
def pi_command(self,msg):
self.cmd_to_send = msg
def spin(self):
encoders = [0,0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
# NEED to reorder the laser scans and flip the laser around... this will not be intuitive for students!!
# things that don't ever change
scan_link = rospy.get_param('~frame_id',ROBOT_NAME+'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=ROBOT_NAME+scan_link))
scan.angle_min = -pi
scan.angle_max = pi
scan.angle_increment = pi/180.0
scan.range_min = 0.020
scan.range_max = 5.0
scan.time_increment = 1.0/(5*360)
scan.scan_time = 0.2
odom = Odometry(header=rospy.Header(frame_id=ROBOT_NAME+"odom"), child_frame_id=ROBOT_NAME+'base_link')
# main loop of driver
r = rospy.Rate(5)
iter_count = 0
rospy.sleep(4)
#self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
#print "spinning"
iter_count += 1
if self.cmd_to_send != None:
self.robot.send_command(self.cmd_to_send)
self.cmd_to_send = None
t_start = time.time()
self.robot.requestScan()
#time.sleep(.01)
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
if delta_t-0.2 > 0.1:
print "Iteration took longer than expected (should be 0.2) ", delta_t
scan.header.stamp = rospy.Time.now()
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
# repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180)
scan.ranges.append(scan.ranges[0])
scan.intensities.append(scan.intensities[0])
#print 'Got scan ranges %f' % (time.time() - t_start)
# get motor encoder values
curr_motor_time = rospy.Time.now()
t_start = time.time()
try:
start_t = rospy.Time.now()
left, right, left_speed, right_speed = self.robot.getMotors()
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
# now update position information
# might consider moving curr_motor_time down
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
d_left = (left - encoders[0])/1000.0
d_right = (right - encoders[1])/1000.0
# might consider using speed instead!
#print d_left, d_right, left_speed*dt/1000.0, right_speed*dt/1000.0
#d_left, d_right = left_speed*dt/1000.0, right_speed*dt/1000.0
encoders = [left, right]
dx = (d_left+d_right)/2
dth = (d_right-d_left)/(BASE_WIDTH/1000.0)
total_dth += dth
x = cos(dth)*dx
y = -sin(dth)*dx
self.x += cos(self.th)*x - sin(self.th)*y
self.y += sin(self.th)*x + cos(self.th)*y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
odom.header.stamp = curr_motor_time
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx/dt
odom.twist.twist.angular.z = dth/dt
self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), curr_motor_time, ROBOT_NAME+"base_link", ROBOT_NAME+"odom" )
self.odomPub.publish(odom)
#print 'Got motors %f' % (time.time() - t_start)
except Exception as err:
print "my error is " + str(err)
t_start = time.time()
self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
try:
bump_sensors = self.robot.getDigitalSensors()
self.bumpPub.publish(Bump(leftFront=bump_sensors[0],leftSide=bump_sensors[1],rightFront=bump_sensors[2],rightSide=bump_sensors[3]))
except:
print "failed to get bump sensors!"
# # send updated movement commands
#print 'Set motors %f' % (time.time() - t_start)
# publish everything
#print "publishing scan!"
self.scanPub.publish(scan)
# wait, then do it again
r.sleep()
def cmdVelCb(self,req):
x = req.linear.x * 1000
th = req.angular.z * (BASE_WIDTH/2)
k = max(abs(x-th),abs(x+th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
self.cmd_vel = [ int(x-th) , int(x+th) ]
class SEMAPEnvironmentServices():
server = None
objects = {}
tf_broadcaster = None
tf_listener = None
marker_pub = None
range_query2d_marker = None
range_query3d_marker = None
range_query_marker = None
area_query_marker = None
volume_query_marker = None
def __init__(self):
self.setup_node()
self.server = InteractiveMarkerServer("semap_environment")
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
def setup_node(self):
rospy.init_node('semap_environment_services')
rospy.loginfo( "SEMAP Environment Services are initializing...\n" )
rospy.Timer(rospy.Duration(0.02), self.publishTF)
#rospy.Timer(rospy.Duration(1.0), self.activate_robot_surroundings)
rospy.Subscriber("create_object", PoseStamped, self.createObjectCb)
#rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinPolygonCb)
#rospy.Subscriber("polygon", PolygonStamped, self.copyObjectWithinPolygonCb)
rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinVolumeCb)
rospy.Subscriber("range2d", PointStamped, self.findObjectWithinRange2DCb)
rospy.Subscriber("range_query", PointStamped, self.findObjectWithinRangeCb)
self.range_query_marker = rospy.Publisher("distance_query", MarkerArray, queue_size=10)
self.area_query_marker = rospy.Publisher("area_query", PolygonStamped, queue_size=10)
self.volume_query_marker = rospy.Publisher("volume_query", MarkerArray, queue_size=10)
self.range_query2d_marker = rospy.Publisher("range_query2d_marker", Marker, queue_size=10)
self.marker_pub = rospy.Publisher("collection_marker", MarkerArray, queue_size=10)
user = rospy.get_param('~user')
password = rospy.get_param('~password')
host = rospy.get_param('~host')
database = rospy.get_param('~database')
initializeConnection(user, password, host, database, False)
## Active Objects
srv_refresh_objects = rospy.Service('refresh_objects', ActivateObjects, self.refresh_objects)
srv_refresh_all_objects = rospy.Service('refresh_all_objects', ActivateAllObjects, self.refresh_all_objects)
srv_activate_objects = rospy.Service('activate_objects', ActivateObjects, self.activate_objects)
srv_activate_all_objects = rospy.Service('activate_all_objects', ActivateAllObjects, self.activate_all_objects)
srv_deactivate_objects = rospy.Service('deactivate_objects', DeactivateObjects, self.deactivate_objects)
srv_deactivate_all_object = rospy.Service('deactivate_all_objects', DeactivateAllObjects, self.deactivate_all_objects)
srv_show_objects = rospy.Service('show_objects', ActivateObjects, self.show_objects)
srv_show_all_objects = rospy.Service('show_all_objects', ActivateAllObjects, self.show_all_objects)
#srv_unshow_objects = rospy.Service('unshow_objects', ActivateObjects, self.activate_all_objects)
#srv_unshow_all_objects = rospy.Service('unshow_all_objects', ActivateAllObjects, self.activate_all_objects)
srv_show_distance_between_objects = rospy.Service('show_distance_between_objects', GetDistanceBetweenObjects, self.showDistanceBetweenObjects)
srv_show_objects_within_range = rospy.Service('show_objects_within_range', GetObjectsWithinRange, self.showObjectWithinRange)
srv_show_objects_within_area = rospy.Service('show_objects_within_area', GetObjectsWithinArea, self.showObjectWithinArea)
srv_show_objects_within_volume = rospy.Service('show_objects_within_volume', GetObjectsWithinVolume, self.showObjectWithinVolume)
srv_activate_objects_in_objects = rospy.Service('activate_objects_in_objects', GetObjectsInObjects, self.activateObjectsInObjects)
srv_show_objects_in_objects = rospy.Service('show_objects_in_objects', GetObjectsInObjects, self.showObjectsInObjects)
srv_activate_objects_on_objects = rospy.Service('activate_objects_on_objects', GetObjectsOnObjects, self.activateObjectsOnObjects)
srv_show_objects_on_objects = rospy.Service('show_objects_on_objects', GetObjectsOnObjects, self.showObjectsOnObjects)
rospy.loginfo( "SEMAP DB Services are running...\n" )
def spin(self):
rospy.spin()
### Services
def activate_objects(self, req):
then = rospy.Time.now()
if len( req.ids) > 0:
rospy.loginfo("SpatialEnv SRVs: activate_objects")
#self.deactivate_objects(req)
get_res = call_get_object_instances(req.ids)
rospy.loginfo("Call Get Object Instances took %f seconds" % (rospy.Time.now() - then).to_sec())
for obj in get_res.objects:
#rospy.loginfo("Activate object: %s" % obj.name)
# get inst visu from db
# if exits: convertNonDBtype
# else create one, store and pass on
if obj.name in self.objects.keys():
#if self.objects[obj.name].status == "active":
#print 'object', obj.name, 'is already active and gets reactivated'
if self.objects[obj.name].status == "inactive":
del self.objects[obj.name]
#print 'object', obj.name, 'is inactive and gets removed from inactive pool'
#else:
# print 'object was unknown so far, now its active'
then2 = rospy.Time.now()
active_object = EnvironmentObject(obj.id, obj.name, obj, InteractiveObjectMarker(obj, self.server, None ), status = "active")
#rospy.loginfo("Marker took %f seconds" % (rospy.Time.now() - then2).to_sec())
self.objects[obj.name] = active_object
#rospy.loginfo("Took %f seconds" % (rospy.Time.now() - now).to_sec())
self.publishTF(None)
rospy.loginfo("Took %f seconds" % (rospy.Time.now() - then).to_sec())
rospy.loginfo("SpatialEnv SRVs: activate_objects - done")
return ActivateObjectsResponse()
def show_objects(self, req):
if len( req.ids) > 0:
rospy.loginfo("SpatialEnv SRVs: show_objects")
get_res = call_get_object_instances(req.ids)
for obj in get_res.objects:
if obj.name in self.objects.keys():
if self.objects[obj.name].status == "active":
print 'object', obj.name, 'is already active'
break
elif self.objects[obj.name].status == "inactive":
print 'object', obj.name, 'is inactive'
ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive")
self.objects[obj.name] = ghost
else:
print 'object was unknown so far, now its inactive'
ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive")
self.objects[obj.name] = ghost
rospy.loginfo("SpatialEnv SRVs: show_objects - done")
return ActivateObjectsResponse()
def refresh_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: reactivate_objects")
print req.ids
active_req = ActivateObjectsRequest()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].id in req.ids:
if self.objects[key].status == "active":
active_req.ids.append( self.objects[key].id )
elif self.objects[key].status == "inactive":
inactive_req.ids.append( self.objects[key].id )
self.activate_objects(active_req)
self.show_objects(inactive_req)
res = ActivateObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: reactivate_objects - done")
return res
def refresh_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects")
active_req = ActivateObjectsRequest()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].status == "active":
active_req.ids.append( self.objects[key].id )
elif self.objects[key].status == "inactive":
inactive_req.ids.append( self.objects[key].id )
self.activate_objects(active_req)
self.show_objects(inactive_req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects - done")
return res
def deactivate_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: deactivate_objects")
res = DeactivateObjectsResponse()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].id in req.ids:
del self.objects[key]
self.show_objects(req)
self.publishTF(None)
return res
def deactivate_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: deactivate_all_objects")
self.objects = {}
self.publishTF(None)
self.show_all_objects(req)
return DeactivateAllObjectsResponse()
def activate_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: activate_all_objects")
ids = db().query(ObjectInstance.id).all()
req = ActivateObjectsRequest()
req.ids = [id for id, in ids]
self.activate_objects(req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: activate_all_objects - done")
return res
def show_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: show_all_objects")
ids = db().query(ObjectInstance.id).all()
req = ActivateObjectsRequest()
req.ids = [id for id, in ids]
self.show_objects(req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: show_all_objects - done")
return res
def publishTF(self, msg):
if len(self.objects) > 0:
for key in self.objects.keys():
if self.objects[key].status == "active":
object = self.objects[key].object
translation = object.pose.pose.position
rotation = object.pose.pose.orientation
time = rospy.Time.now()
#print 'publish tf'
self.tf_broadcaster.sendTransform( (translation.x, translation.y, translation.z), \
(rotation.x, rotation.y, rotation.z, rotation.w), \
time, object.name, object.pose.header.frame_id)
def activate_robot_surroundings(self, msg):
#if activate_robot_surroundings:
try:
(trans,rot) = self.tf_listener.lookupTransform('/world', '/base_link', rospy.Time(0))
print 'current robot position', trans
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'could not determine current robot position'
#else:
# robot inactive
def createObjectCb(self, pose):
print 'Create Object from RViz pose'
app = QApplication(sys.argv)
widget = ChooseObjectDescriptionWidget(0)
widget.show()
app.exec_()
desc_name, desc_id = widget.getChoice()
del app, widget
if(desc_id == -1):
new_desc = ROSObjectDescription()
new_desc.type = desc_name
res = call_add_object_descriptions( [ new_desc ] )
print res
desc_id = res.ids[0]
obj = ROSObjectInstance()
obj.description.id = desc_id
obj.pose = pose
res = call_add_object_instances( [obj] )
call_activate_objects( res.ids )
def activateObjectsInObjects(self, req):
res = call_get_objects_in_objects(req.reference, req.target, req.fully_within)
ids = []
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
return res
def showObjectsInObjects(self, req):
res = call_get_objects_in_objects(req.reference, req.target, req.fully_within)
ids = []
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_show_objects( ids )
return res
def activateObjectsOnObjects(self, req):
print 'GOT SO FAR'
res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold)
print 'NOW WHAT'
ids = []
for pair in res.pairs:
#if pair.reference_id not in ids:
# ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
return res
def showObjectsOnObjects(self, req):
print 'GOT SO FAR'
res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold)
print 'NOW WHAT'
ids = []
for pair in res.pairs:
#if pair.reference_id not in ids:
# ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_show_objects( ids )
return res
def findObjectWithinRange2DCb(self, point_stamped):
distance = 2.0
print 'find Objects Within Range2D'
#res = call_get_objects_within_range2d([], "Position2D", point_stamped.point, distance, fully_within = False)
#res = call_get_objects_within_range2d([], "FootprintBox", point_stamped.point, distance, fully_within = False)
#res = call_get_objects_within_range2d(["ConferenceTable", "ConferenceChair"], "Position2D", point_stamped.point, 3.0)
#call_activate_objects( res.ids )
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "2d_range_query_marker"
marker.id = 0
marker.type = 3 #cylinder
marker.scale.x = distance*2
marker.scale.y = distance*2
marker.scale.z = 0.01
marker.pose.position = point_stamped.point
marker.pose.position.z = 0.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.5
self.range_query2d_marker.publish(marker)
def findObjectWithinRangeCb(self, point_stamped):
distance = 1.5
res = call_get_objects_within_range(point_stamped.point, [], "FootprintHull", distance, fully_within = False)
ids = []
array = MarkerArray()
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "range"
marker.id = 0
marker.type = 3 #sphere
marker.scale.x = distance*2
marker.scale.y = distance*2
marker.scale.z = 0.005
marker.pose.position = point_stamped.point
marker.pose.position.z = 0.0
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.75
marker.color.a = 0.5
array.markers.append(marker)
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
def showObjectWithinRange(self, req):
res = call_get_objects_within_range(req.reference_point, req.target_object_types, req.target_object_geometry_type,
req.distance, req.fully_within)
ids = []
array = MarkerArray()
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "range"
marker.id = 0
marker.type = 3 #sphere
marker.scale.x = req.distance*2
marker.scale.y = req.distance*2
marker.scale.z = 0.005
marker.pose.position = req.reference_point
marker.pose.position.z = 0.0
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.75
marker.color.a = 0.5
array.markers.append(marker)
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
return res
def findObjectWithinVolumeCb(self, polygon_stamped):
extrude = call_extrude_polygon(polygon_stamped.polygon, 0.0, 0.0, 0.9)
mesh = PolygonMeshStamped()
mesh.header.frame_id = "world"
mesh.mesh = extrude.mesh
print extrude.mesh
res = call_get_objects_within_volume(extrude.mesh, ['Mug','Teapot','Monitor','Keyboard'], 'BoundingBox', True)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
array = MarkerArray()
pose = ROSPoseStamped()
pose.header.frame_id = 'world'
for polygon in extrude.mesh.polygons:
poly = Polygon()
for point in polygon.vertex_indices:
poly.points.append(extrude.mesh.vertices[point])
geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly)
array.markers.append(geo_marker)
id = 0
for m in array.markers:
m.header.frame_id = 'world'
m.id = id
id += 1
self.volume_query_marker.publish(array)
def showObjectWithinVolume(self, req):
res = call_get_objects_within_volume(req.reference_mesh, req.target_object_types, req.target_object_geometry_type, req.fully_within)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
array = MarkerArray()
pose = ROSPoseStamped()
pose.header.frame_id = 'world'
for polygon in req.reference_mesh.polygons:
poly = Polygon()
for point in polygon.vertex_indices:
poly.points.append(req.reference_mesh.vertices[point])
geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly)
array.markers.append(geo_marker)
id = 0
for m in array.markers:
m.header.frame_id = 'world'
m.id = id
id += 1
self.volume_query_marker.publish(array)
return res
def findObjectWithinPolygonCb(self, polygon_stamped):
res = call_get_objects_within_area(polygon_stamped.polygon, ["Chair"], "Position2D", fully_within = False)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
self.area_query_marker.publish(polygon_stamped)
return res
def copyObjectWithinPolygonCb(self, polygon_stamped):
res = call_get_objects_within_area(polygon_stamped.polygon, [], "Position2D", fully_within = False)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
print 'COPY'
copy_res = call_copy_object_instances(ids)
if len( copy_res.ids ) > 1:
print 'copy_res', copy_res
obj_res = call_get_object_instances( [copy_res.ids[0]] )
print 'bind to ', obj_res.objects[0].name
frame = obj_res.objects[0].name
for id in copy_res.ids:
if id != obj_res.objects[0].id:
print 'bind', id, 'to', obj_res.objects[0].id
call_change_frame(id, frame, False)
call_activate_objects( copy_res.ids )
call_refresh_objects( copy_res.ids )
return res
def showObjectWithinArea(self, req):
res = call_get_objects_within_area(req.reference_polygon, req.target_object_types, req.target_object_geometry_type, req.fully_within)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
polygon_stamped = PolygonStamped()
polygon_stamped.header.frame_id = "world"
polygon_stamped.polygon = req.reference_polygon
self.area_query_marker.publish(polygon_stamped)
return res
def showDistanceBetweenObjects(self, req):
res = call_get_distance_between_objects(req.reference_object_types, req.reference_object_geometry_type,
req.target_object_types, req.target_object_geometry_type,
req.min_range, req.max_range,
req.sort_descending, req.max_distance, return_points = True)
array = MarkerArray()
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
ids =[]
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
return res
def list_active_objects(self, msg):
for key in self.objects.keys():
active = active_objects[key]
print 'ID :', active.id
print 'TYPE :', active.object.description.type
print 'NAME :', active.object.name
print 'ALIAS :', active.object.alias
print 'POSE :', active.object.pose
def monitor(self):
''' This function starts the robucar_mon node and begin publishing
the received data to robot_data using RobotDataStamped msg'''
pub = rospy.Publisher('robot_data', RobotDataStamped, queue_size=1)
odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
tf_broadcaster = TransformBroadcaster()
odom = Odometry(
header=rospy.Header(frame_id="odom"), child_frame_id='base_link')
self.then = rospy.Time.now()
while not rospy.is_shutdown():
buf = self.conn.recv(struct.calcsize(self.ctrl_def))
self.data = struct.unpack(self.ctrl_def, buf)
self.now = rospy.Time.now()
v = self.data[0]
phi = radians(self.data[5])
if (self.now - self.then).to_sec() < 0.095:
pass
else:
dt = (self.now - self.then).to_sec()
if self.mode == 0:
dx = v * cos(self.th)
dy = v * sin(self.th)
dth = (v / self.L) * tan(phi)
elif self.mode == 1:
dx = v * cos(self.th + phi)
dy = v * sin(self.th + phi)
dth = (2 * v / self.L) * sin(phi)
elif self.mode == 2:
dx = v * cos(self.th)
dy = v * sin(self.th)
dth = 0.0
self.x += dx * dt
self.y += dy * dt
self.th += dth * dt
quaternion = Quaternion(*quaternion_from_euler(0, 0, self.th))
odom.header.stamp = self.now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = dy
odom.twist.twist.angular.z = dth
self.stampedData.header.stamp = rospy.Time.now()
self.stampedData.data.speed_average = self.data[0]
self.stampedData.data.speed_FL = self.data[1]
self.stampedData.data.speed_FR = self.data[2]
self.stampedData.data.speed_RL = self.data[3]
self.stampedData.data.speed_RR = self.data[4]
self.stampedData.data.angle_forward = self.data[5]
self.stampedData.data.angle_rear = self.data[6]
self.stampedData.data.position_pan = self.data[7]
self.stampedData.data.position_tilt = self.data[8]
self.stampedData.data.speed_pan = self.data[9]
self.stampedData.data.speed_tilt = self.data[10]
pub.publish(self.stampedData)
odom_pub.publish(odom)
tf_broadcaster.sendTransform((self.x, self.y, 0),
(quaternion.x, quaternion.y,
quaternion.z, quaternion.w),
self.now,
"base_link",
"odom")
# qfix = Quaternion(*quaternion_from_euler(0, 0, 0))
# tf_broadcaster.sendTransform((0, 0, 0),
# (qfix.x, qfix.y, qfix.z, qfix.w),
# self.now,
# "odom",
# "map")
self.then = self.now
class OMOR1miniNode:
def __init__(self):
self.ph = PacketHandler()
self.ph.ser.reset_input_buffer()
self.ph.ser.reset_output_buffer()
self.ph.stop_periodic_comm()
self.calc_yaw = ComplementaryFilter()
self.max_lin_vel_x = 1.2
self.max_ang_vel_z = 1.0
self.ph.robot_state = {
"VW": [0., 0.],
"ODO": [0., 0.],
"ACCL": [0., 0., 0.],
"GYRO": [0., 0., 0.],
"POSE": [0., 0.],
"BAT": [0., 0., 0.],
}
self.ph.incomming_info = ['ODO', 'VW', "GYRO", "ACCL"]
self.odom_pose = OdomPose()
self.odom_vel = OdomVel()
self.joint = Joint()
self.gear_ratio = rospy.get_param("/motor_spec/gear_ratio") / 10.
self.wheel_base = rospy.get_param("/motor_spec/wheel_base")
self.wheel_radius = rospy.get_param("/motor_spec/wheel_radius")
self.enc_pulse = rospy.get_param("/motor_spec/enc_pulse")
self.use_gyro = rospy.get_param("/use_imu_during_odom_calc/use_imu")
self.calc_yaw.filter_coef = rospy.get_param(
"/use_imu_during_odom_calc/complementary_filter_coef")
self.distance_per_pulse = 2 * math.pi * self.wheel_radius / self.enc_pulse / self.gear_ratio
rospy.loginfo('Robot GEAR ratio: %s', self.gear_ratio)
rospy.loginfo('Robot wheel_base: %s', self.wheel_base)
rospy.loginfo('Robot wheel_radius: %s', self.wheel_radius)
rospy.loginfo('Robot enc_pulse: %s', self.enc_pulse)
rospy.loginfo('Robot distance_per_pulse: %s', self.distance_per_pulse)
rospy.Service('battery_status', Battery, self.battery_service_handle)
rospy.Service('set_led_color', Color, self.led_color_service_handle)
rospy.Service('save_led_color', Color,
self.save_led_color_service_handle)
rospy.Service('reset_odom', ResetOdom, self.reset_odom_handle)
rospy.Subscriber("cmd_vel", Twist, self.sub_cmd_vel, queue_size=1)
self.pub_joint_states = rospy.Publisher('joint_states',
JointState,
queue_size=10)
self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odom_broadcaster = TransformBroadcaster()
self.ph.update_battery_state()
self.ph.set_periodic_info()
sleep(0.1)
self.ph.set_periodic_info()
rospy.loginfo('==> Start R1mini ')
rospy.Timer(rospy.Duration(0.01), self.update_robot)
self.odom_pose.timestamp = rospy.Time.now()
self.odom_pose.pre_timestamp = rospy.Time.now()
def convert2odo_from_each_wheel(self, enc_l, enc_r):
return enc_l * self.distance_per_pulse, enc_r * self.distance_per_pulse
def update_odometry(self, odo_l, odo_r, trans_vel, orient_vel, vel_z):
odo_l /= 1000.
odo_r /= 1000.
trans_vel /= 1000.
orient_vel /= 1000.
self.odom_pose.timestamp = rospy.Time.now()
dt = (self.odom_pose.timestamp - self.odom_pose.pre_timestamp).to_sec()
self.odom_pose.pre_timestamp = self.odom_pose.timestamp
if self.use_gyro:
self.calc_yaw.wheel_ang += orient_vel * dt
self.odom_pose.theta = self.calc_yaw.calc_filter(
vel_z * math.pi / 180., dt)
rospy.loginfo(
'R1mini state : whl pos %1.2f, %1.2f, gyro : %1.2f, whl odom : %1.2f, robot theta : %1.2f',
odo_l, odo_r, vel_z, self.calc_yaw.wheel_ang * 180 / math.pi,
self.odom_pose.theta * 180 / math.pi)
else:
self.odom_pose.theta += orient_vel * dt
rospy.loginfo('R1mini state : wheel pos %s, %s, speed %s, %s',
odo_l, odo_r, trans_vel, orient_vel)
d_x = trans_vel * math.cos(self.odom_pose.theta)
d_y = trans_vel * math.sin(self.odom_pose.theta)
self.odom_pose.x += d_x * dt
self.odom_pose.y += d_y * dt
odom_orientation_quat = quaternion_from_euler(0, 0,
self.odom_pose.theta)
self.odom_vel.x = trans_vel
self.odom_vel.y = 0.
self.odom_vel.w = orient_vel
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_footprint"
self.odom_broadcaster.sendTransform(
(self.odom_pose.x, self.odom_pose.y, 0.), odom_orientation_quat,
self.odom_pose.timestamp, odom.child_frame_id,
odom.header.frame_id)
odom.header.stamp = rospy.Time.now()
odom.pose.pose = Pose(Point(self.odom_pose.x, self.odom_pose.y, 0.),
Quaternion(*odom_orientation_quat))
odom.twist.twist = Twist(Vector3(self.odom_vel.x, self.odom_vel.y, 0),
Vector3(0, 0, self.odom_vel.w))
self.odom_pub.publish(odom)
def updateJointStates(self, odo_l, odo_r, trans_vel, orient_vel):
odo_l /= 1000.
odo_r /= 1000.
wheel_ang_left = odo_l / self.wheel_radius
wheel_ang_right = odo_r / self.wheel_radius
wheel_ang_vel_left = (
trans_vel -
(self.wheel_base / 2.0) * orient_vel) / self.wheel_radius
wheel_ang_vel_right = (
trans_vel +
(self.wheel_base / 2.0) * orient_vel) / self.wheel_radius
self.joint.joint_pos = [wheel_ang_left, wheel_ang_right]
self.joint.joint_vel = [wheel_ang_vel_left, wheel_ang_vel_right]
joint_states = JointState()
joint_states.header.frame_id = "base_link"
joint_states.header.stamp = rospy.Time.now()
joint_states.name = self.joint.joint_name
joint_states.position = self.joint.joint_pos
joint_states.velocity = self.joint.joint_vel
joint_states.effort = []
self.pub_joint_states.publish(joint_states)
def update_robot(self, event):
raw_data = self.ph.parser()
try:
[trans_vel, orient_vel] = self.ph.robot_state['VW']
[odo_l, odo_r] = self.ph.robot_state['ODO']
[vel_x, vel_y, vel_z] = self.ph.robot_state['GYRO']
[acc_x, acc_y, acc_z] = self.ph.robot_state['ACCL']
self.update_odometry(odo_l, odo_r, trans_vel, orient_vel, vel_z)
self.updateJointStates(odo_l, odo_r, trans_vel, orient_vel)
except ValueError:
rospy.logwarn(
"ValueError occupied during read robot status in update_robot state. \n\r Raw_data : %s",
raw_data)
def sub_cmd_vel(self, cmd_vel_msg):
lin_vel_x = cmd_vel_msg.linear.x
ang_vel_z = cmd_vel_msg.angular.z
lin_vel_x = max(-self.max_lin_vel_x, min(self.max_lin_vel_x,
lin_vel_x))
ang_vel_z = max(-self.max_ang_vel_z, min(self.max_ang_vel_z,
ang_vel_z))
self.ph.set_wheel_velocity(lin_vel_x * 1000, ang_vel_z * 1000)
def battery_service_handle(self, req):
self.ph.update_battery_state()
bat_status = self.ph.robot_state['BAT']
if len(bat_status) == 3:
volt = bat_status[0] * 0.1
SOC = bat_status[1]
current = bat_status[2] * 0.001
return BatteryResponse(volt, SOC, current)
else:
rospy.logwarn("Battery Status is not correct.")
def led_color_service_handle(self, req):
command = "$cCOLOR," + str(req.red) + ',' + str(req.green) + ',' + str(
req.blue)
self.ph.write_port(command)
return ColorResponse()
def save_led_color_service_handle(self, req):
command = "$sCOLOR," + str(req.red) + ',' + str(req.green) + ',' + str(
req.blue)
self.ph.write_port(command)
return ColorResponse()
def reset_odom_handle(self, req):
self.odom_pose.x = req.x
self.odom_pose.y = req.y
self.odom_pose.theta = req.theta
return ResetOdomResponse()
def main(self):
rospy.spin()
class base_controller(Thread):
""" Controller to handle movement & odometry feedback for a differential drive mobile base. """
def __init__(self, Serializer, name):
Thread.__init__ (self)
self.finished = Event()
# Handle for the Serializer
self.mySerializer = Serializer
# Parameters
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.ticks_per_meter = float(self.mySerializer.ticks_per_meter)
self.wheel_track = self.mySerializer.wheel_track
self.gear_reduction = self.mySerializer.gear_reduction
self.encoder_resolution = self.mySerializer.encoder_resolution
now = rospy.Time.now()
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = now + self.t_delta
self.then = now # time for determining dx/dy
# internal data
self.enc_left = 0 # encoder readings
self.enc_right = 0
self.x = 0. # position in xy plane
self.y = 0.
self.th = 0. # rotation in radians
self.encoder_error = False
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.mySerializer.clear_encoder([1, 2])
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started Base Controller '"+ name +"' for a base of " + str(self.wheel_track) + "m wide with " + str(self.ticks_per_meter) + " ticks per meter")
def run(self):
rosRate = rospy.Rate(self.rate)
rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")
old_left = old_right = 0
bad_encoder_count = 0
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
# read encoders
try:
left, right = self.mySerializer.get_encoder_count([1, 2])
except:
self.encoder_error = True
rospy.loginfo("Could not read encoders: " + str(bad_encoder_count))
bad_encoder_count += 1
rospy.loginfo("Encoder counts at exception: " + str(left) + ", " + str(right))
continue
if self.encoder_error:
self.enc_left = left
self.enc_right = right
self.encoder_error = False
continue
# calculate odometry
dleft = (left - self.enc_left) / self.ticks_per_meter
dright = (right - self.enc_right) / self.ticks_per_meter
self.enc_left = left
self.enc_right = right
dxy_ave = (dleft + dright) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
def cmdVelCallback(self, req):
""" Handle velocity-based movement requests. """
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
# Set motor speeds in meters per second.
self.mySerializer.mogo_m_per_s([1, 2], [left, right])
def stop(self):
print "Shutting down base controller"
self.finished.set()
self.join()
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param(
'~rate', 10) # the rate at which to publish the transform
self.base_width = float(rospy.get_param(
'~base_width', 0.245)) # The wheel base width in meters
self.base_frame_id = rospy.get_param(
'~base_frame_id',
'base_link') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param(
'~odom_frame_id',
'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param(
'wheel_low_wrap',
(self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
self.encoder_high_wrap = rospy.get_param(
'wheel_high_wrap',
(self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
self.v_left = 0
self.v_right = 0
self.d_left = 0
self.d_right = 0
self.v_left_update = False
self.v_right_update = False
self.d_left_update = False
self.d_right_update = False
# subscriptions
rospy.Subscriber("lwheel_vel", Float32, self.lwheelCallback)
rospy.Subscriber("rwheel_vel", Float32, self.rwheelCallback)
rospy.Subscriber("lwheel_distance", Float32, self.lwheeldCallback)
rospy.Subscriber("rwheel_distance", Float32, self.rwheeldCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
def lwheeldCallback(self, msg):
self.d_left_update = True
self.d_left = msg.data
def rwheeldCallback(self, msg):
self.d_right_update = True
self.d_right = msg.data
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if self.d_left_update and self.d_right_update and self.v_left_update and self.v_right_update:
# distance traveled is the average of the two wheels
d = (self.d_left + self.d_right) / 2
# this approximation works (in radians) for small angles
th = (self.d_right - self.d_left) / self.base_width
# calculate velocities
self.dx = (self.v_left + self.v_right) / 2
self.dr = (self.v_left - self.v_right) / self.base_width
if (d != 0):
# calculate distance traveled in x and y
x = cos(th) * d
y = -sin(th) * d
# calculate the final position of the robot
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
self.v_left_update = True
self.v_right_update = True
self.d_left_update = True
self.d_right_update = True
#############################################################################
def lwheelCallback(self, msg):
#############################################################################
# update left wheel velocity
self.v_left_update = True
self.v_left = msg.data
#############################################################################
def rwheelCallback(self, msg):
#############################################################################
# update right wheel velocity
self.v_right_update = True
self.v_right = msg.data
class HueyNode:
def __init__(self):
""" Start up connection to the Huey Robot. """
rospy.init_node('neato')
self.CMD_RATE =2
self.newTwistCmd = 0
self.telnet_ip = rospy.get_param('~telnet_ip', "Huey-Tiger")
rospy.loginfo("Using telnet: %s" % self.telnet_ip)
self.robot = Huey(self.telnet_ip)
#rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.buttonPub = rospy.Publisher('button', Button, queue_size=10)
self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#self.cmd_vel = [0, 0]
#self.old_vel = self.cmd_vel
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
#rospy.loginfo("spin: before LaserScan")
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min =0.0
scan.angle_max =359.0*pi/180.0
scan.angle_increment =pi/180.0
scan.range_min = 0.020
scan.range_max = 5.0
#rospy.loginfo("spin: before Odometry")
odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')
button = Button()
sensor = Sensor()
# main loop of driver
r = rospy.Rate(2)
cmd_rate= self.CMD_RATE
rospy.loginfo(">>> spin: before while loop <<<")
while not rospy.is_shutdown():
left, right = self.robot.getMotors()
# prepare laser scan
scan.header.stamp = rospy.Time.now()
self.robot.requestScan()
#rospy.loginfo("spin: loop: requestScan")
scan.ranges = self.robot.getLdsScan()
#if len(scan.ranges) == 0:
# scan.ranges = self.robot.getLdsScan()
# now update position information
dt = (scan.header.stamp - then).to_sec()
then = scan.header.stamp
d_left = (left - encoders[0])/1000.0
d_right = (right - encoders[1])/1000.0
encoders = [left, right]
#print d_left, d_right, encoders
dx = (d_left+d_right)/2
dth = (d_right-d_left)/(self.robot.base_width/1000.0)
x = cos(dth)*dx
y = -sin(dth)*dx
self.x += cos(self.th)*x - sin(self.th)*y
self.y += sin(self.th)*x + cos(self.th)*y
self.th += dth
#print self.x,self.y,self.th
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx/dt
odom.twist.twist.angular.z = dth/dt
# sensors
lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
#rospy.loginfo("spin: loop: getDigitalSensors")
# buttons
btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()
# publish everything
self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
quaternion.w), then, "base_footprint", "odom")
#rospy.loginfo("spin: loop: sendTransform")
if len(scan.ranges) > 0:
self.scanPub.publish(scan)
self.odomPub.publish(odom)
button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
if b == 1:
button.value = b
button.name = button_enum[idx]
self.buttonPub.publish(button)
for idx, b in enumerate((lsb, rsb, lfb, rfb)):
if b == 1:
sensor.value = b
sensor.name = sensor_enum[idx]
self.sensorPub.publish(sensor)
# wait, then do it again
#rospy.loginfo("spin: loop: before sleep()")
r.sleep()
# Steve: testing with longer sleep
#time.sleep(1)
# shut down
rospy.loginfo(">>>>>> Exiting <<<<<<<<")
class OdometryNode:
def __init__(self):
self.odometry = odometry.Odometry()
def main(self):
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.tfPub = TransformBroadcaster()
rospy.init_node('diff_drive_odometry')
self.nodeName = rospy.get_name()
rospy.loginfo("{0} started".format(self.nodeName))
rospy.Subscriber("lwheel_ticks", Int32, self.leftCallback)
rospy.Subscriber("rwheel_ticks", Int32, self.rightCallback)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.on_initial_pose)
self.ticksPerMeter = int(rospy.get_param('~ticks_per_meter'))
self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
self.rate = float(rospy.get_param('~rate', 10.0))
self.baseFrameID = rospy.get_param('~base_frame_id', 'base_link')
self.odomFrameID = rospy.get_param('~odom_frame_id', 'odom')
self.encoderMin = int(rospy.get_param('~encoder_min', -32768))
self.encoderMax = int(rospy.get_param('~encoder_max', 32767))
self.odometry.setWheelSeparation(self.wheelSeparation)
self.odometry.setTicksPerMeter(self.ticksPerMeter)
self.odometry.setEncoderRange(self.encoderMin, self.encoderMax)
self.odometry.setTime(rospy.get_time())
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.publish()
rate.sleep()
def publish(self):
self.odometry.updatePose(rospy.get_time())
now = rospy.get_rostime()
pose = self.odometry.getPose()
q = quaternion_from_euler(0, 0, pose.theta)
self.tfPub.sendTransform((pose.x, pose.y, 0), (q[0], q[1], q[2], q[3]),
now, self.baseFrameID, self.odomFrameID)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odomFrameID
odom.child_frame_id = self.baseFrameID
odom.pose.pose.position.x = pose.x
odom.pose.pose.position.y = pose.y
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
odom.twist.twist.linear.x = pose.xVel
odom.twist.twist.angular.z = pose.thetaVel
self.odomPub.publish(odom)
def on_initial_pose(self, msg):
q = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.x, msg.pose.pose.orientation.w
]
roll, pitch, yaw = euler_from_quaternion(q)
pose = Pose()
pose.x = msg.pose.pose.position.x
pose.y = msg.pose.pose.position.y
pose.theta = yaw
rospy.loginfo('Setting initial pose to %s', pose)
self.odometry.setPose(pose)
def leftCallback(self, msg):
self.odometry.updateLeftWheel(msg.data)
def rightCallback(self, msg):
self.odometry.updateRightWheel(msg.data)
class ThymioDriver():
# ======== class initialization ========
def __init__(self):
rospy.init_node('thymio')
# initialize parameters
self.x = 0
self.y = 0
self.th = 0
self.then = rospy.Time.now()
self.odom = Odometry(header=rospy.Header(frame_id='odom'),child_frame_id='base_link')
# load script on the Thymio
rospy.wait_for_service('/aseba/load_script')
load_script = rospy.ServiceProxy('/aseba/load_script',LoadScripts)
script_filename = roslib.packages.get_pkg_dir('thymio_navigation_driver') + '/aseba/thymio_ros.aesl'
load_script(script_filename)
# subscribe to topics
rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)
rospy.Subscriber('/aseba/events/odometry', AsebaEvent, self.on_aseba_odometry_event)
self.aseba_pub = rospy.Publisher('/aseba/events/set_speed', AsebaEvent)
self.odom_pub = rospy.Publisher('odom',Odometry)
self.odom_broadcaster = TransformBroadcaster()
# ======== we send the speed to the aseba running on the robot ========
def set_speed(self,values):
self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(),0,values))
# ======== stop the robot safely ========
def shutdown(self):
self.set_speed([0,0])
# ======== processing odometry events received from the robot ========
def on_aseba_odometry_event(self,data):
now = data.stamp
dt = (now-self.then).to_sec()
self.then = now
dsl = (data.data[0]*dt)/SPEED_COEF # left wheel delta in mm
dsr = (data.data[1]*dt)/SPEED_COEF # right wheel delta in mm
ds = ((dsl+dsr)/2.0)/1000.0 # robot traveled distance in meters
dth = atan2(dsr-dsl,BASE_WIDTH) # turn angle
self.x += ds*cos(self.th+dth/2.0)
self.y += ds*sin(self.th+dth/2.0)
self.th+= dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
self.odom.header.stamp = rospy.Time.now() # OR TO TAKE ONE FROM THE EVENT?
self.odom.pose.pose.position.x = self.x
self.odom.pose.pose.position.y = self.y
self.odom.pose.pose.position.z = 0
self.odom.pose.pose.orientation = quaternion
self.odom.twist.twist.linear.x = ds/dt
self.odom.twist.twist.angular.z = dth/dt
# publish odometry
self.odom_broadcaster.sendTransform((self.x,self.y,0),(quaternion.x,quaternion.y,quaternion.z,quaternion.w),self.then,"base_link","odom")
self.odom_pub.publish(self.odom)
# ======== processing events received from the robot ========
def on_cmd_vel(self,data):
x = data.linear.x * 1000.0 # from meters to millimeters
x = x * SPEED_COEF # to thymio units
th = data.angular.z * (BASE_WIDTH/2) # in mm
th = th * SPEED_COEF # in thymio units
k = max(abs(x-th),abs(x+th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
self.set_speed([int(x-th),int(x+th)])
# ======== ======== ======== ======== ======== ======== ========
def control_loop(self):
rospy.on_shutdown(self.shutdown) # on shutdown hook
while not rospy.is_shutdown():
rospy.spin()
class BoardController():
"""
Main board controller
"""
# List of commands and argument formats
commands = [["cmd_set_motor_params", "ffff"],
["cmd_set_cps_pid_params", "fff"], ["cmd_set_motor_pwm", "ii"],
["cmd_get_motor_pwm", ""], ["cmd_ret_motor_pwm", "ii"],
["cmd_get_encoder_ticks", ""], ["cmd_ret_encoder_ticks", "ll"],
["cmd_set_motor_cps", "ff"], ["cmd_get_motor_cps", ""],
["cmd_ret_motor_cps", "ff"], ["cmd_get_odometry", ""],
["cmd_ret_odometry", "fff"], ["cmd_reset_system", ""],
["cmd_ret_line", "s"]]
def __init__(self, motor_cpr, wheel_radius, wheel_distance, max_linear_vel,
max_angular_vel, odom_rate, base_frame, odom_frame):
"""
Input:
motor_cpr:
motor encoder counts per revolution (4x resolution)
wheel_radius:
radius of the robot wheels (meters)
wheel_distance:
distance between robot wheels (meters)
max_linear_vel:
maximun speed of the robot (m/s)
max_angular_vel:
maximun angular speed of the robot (rad/s)
odom_rate:
publishing rate of robot odometry
base_frame:
robot base frame name
odom_frame:
odometry frame name
"""
self.motor_cpr = motor_cpr
self.wheel_radius = wheel_radius
self.wheel_distance = wheel_distance
self.max_linear_vel = max_linear_vel
self.max_angular_vel = max_angular_vel
self.odom_rate = odom_rate
self.base_frame = base_frame
self.odom_frame = odom_frame
# Initialize ArduinoBoard instance
self.port = self.getBoardUSBPort(self._DEFAULT_PORT_PREFIX)
if len(self.port) == 0:
rospy.logfatal("Not founded any ttyACM port. Exiting...")
exit()
rospy.loginfo("Connecting to port %s..." % self.port[0])
# Arduino Zero >> int 4 bytes
self.board = PyCmdMessenger.ArduinoBoard(self.port[0],
self._DEFAULT_BAUD_RATE,
int_bytes=4)
# Initialize the messenger
self.cmd = PyCmdMessenger.CmdMessenger(self.board,
BoardController.commands)
rospy.loginfo("done. Connecction to serial port %s established" %
self.port[0])
# Reset board
rospy.loginfo("Resetting board counters")
self.cmd.send("cmd_reset_system")
# ROS Node Subscriber
self.sub_cmd_vel = rospy.Subscriber('cmd_vel',
Twist,
self.compute_motor_velocities_cb,
queue_size=1)
# Odometry publisher
self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odom_broadcaster = TransformBroadcaster()
rospy.Timer(rospy.Duration(1.0 / self.odom_rate), self.update_odometry)
def getBoardUSBPort(self, port_prefix):
# Try to find the associated USB COM port
comports = [tuple(p) for p in list(serial.tools.list_ports.comports())]
# List of ports containing port_prefix
port = [port[0] for port in comports if port_prefix in port[0]]
return port
def compute_motor_velocities_cb(self, twist_msg):
"""
Gets /cmd_vel values and compute the appropriate motor velocities
in c.p.s. to send to the robot base
Vr = (2*linear_vel + angular_vel*wheel_distance)/2*wheel_radius
Vl = (2*linear_vel - angular_vel*wheel_distance)/2*wheel_radius
"""
linear_vel = twist_msg.linear.x
angular_vel = twist_msg.angular.z
# Right and Left motor velocities in rad/s
v2 = 2 * linear_vel
r2 = 2 * self.wheel_radius
wL = angular_vel * self.wheel_distance
vr = (v2 - wL) / r2
vl = (v2 + wL) / r2
# Compute velocities in c.p.s
rads_to_counts = self.motor_cpr / (2 * math.pi)
vr *= rads_to_counts
vl *= rads_to_counts
# Send Command
#rospy.loginfo("vr: %f vl: %f" % (vr,vl))
self.cmd.send("cmd_set_motor_cps", vr, vl)
def update_odometry(self, event):
"""
Gets the current odometry from the robot and publish the ROS transform
"""
# send command
self.cmd.send("cmd_get_odometry")
# receive response from Arduino
data = self.cmd.receive()
x = data[1][0]
y = data[1][1]
theta = data[1][2]
now = rospy.Time.now()
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(theta / 2)
quaternion.w = math.cos(theta / 2)
self.odom_broadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), now,
self.base_frame, self.odom_frame)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame
odom.twist.twist.linear.x = 0 # linear velocity
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = 0 # angular velocity
self.odom_pub.publish(odom)
class TFMarkerServer(object):
"""TFMarkerServer"""
def __init__(self, rate = 30.0, filename = None):
# Marker server
self.server = InteractiveMarkerServer('camera_marker')
# TF broadcaster
self.tf = TransformBroadcaster()
# Marker pose
self.pose_mutex = Lock()
self.marker_position = (0.0, 0.0, 0.0)
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Load init position
self.filename = filename
if self.filename:
with open(self.filename, 'r') as stream:
init_data = yaml.load(stream)['init_position']
self.marker_position = (init_data['x'], init_data['y'], init_data['z'])
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Register shutdown callback
rospy.on_shutdown(self.shutdown)
# Add marker
self.add_6DOF()
# Timer for TF broadcaster
rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)
def add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'):
marker = InteractiveMarker()
marker.header.frame_id = frame_id
marker.pose.position = init_position
marker.scale = 0.3
marker.name = 'camera_marker'
marker.description = 'Camera 6-DOF pose control'
# X axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# X axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Y axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# Y axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Z axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# Z axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Add marker to server
self.server.insert(marker, self.marker_feedback)
self.server.applyChanges()
def publish_transform(self, timer_event):
time = rospy.Time.now()
self.pose_mutex.acquire()
self.tf.sendTransform(self.marker_position, self.marker_orientation,
time, 'sensor_base', 'map')
self.pose_mutex.release()
def marker_feedback(self, feedback):
rospy.loginfo('Feedback from ' + feedback.marker_name)
# Check event
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.loginfo( 'Pose changed')
# Update marker position
self.pose_mutex.acquire()
self.marker_position = (feedback.pose.position.x,
feedback.pose.position.y, feedback.pose.position.z)
# Update marker orientation
self.marker_orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y, feedback.pose.orientation.z,
feedback.pose.orientation.w)
self.pose_mutex.release()
def shutdown(self):
data = {
'init_position' :
{
'x': 0.0,
'y': 0.0,
'z': 0.0,
'roll': 0.0,
'pitch': 0.0,
'yaw': 0.0,
}
}
rospy.loginfo('Writing current position')
with open(self.filename, 'w') as outfile:
outfile.write(yaml.dump(data, default_flow_style=True))
class base_controller(Thread):
""" Controller to handle movement & odometry feedback for a differential drive mobile base. """
def __init__(self, name):
Thread.__init__ (self)
self.finished = Event()
# Parameters
self.rate = float(rospy.get_param("~base_controller_rate", 10))
now = rospy.Time.now()
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = now + self.t_delta
self.then = now # time for determining dx/dy
# internal data
self.x = 0. # position in xy plane
self.y = 0.
self.th = 0. # rotation in radians
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started Odometry simmulator " + name )
def run(self):
rosRate = rospy.Rate(self.rate)
rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")
vx = 0.0
vy = 0.0
vth = 0.0
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
def stop(self):
print "Shutting down base odom_sim"
self.finished.set()
self.join()
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.port = rospy.get_param('~port', "/dev/ttyACM0")
rospy.loginfo("Using port: %s"%(self.port))
self.robot = xv11(self.port)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('base_scan', LaserScan)
self.odomPub = rospy.Publisher('odom',Odometry)
self.dropPub = rospy.Publisher('neato_drop',NeatoDropSensor)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = [0,0]
def spin(self):
encoders = [0,0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
# things that don't ever change
scan_link = rospy.get_param('~frame_id','base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0
scan.angle_max = 6.26
scan.angle_increment = 0.017437326
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(5)
self.robot.requestScan()
while not rospy.is_shutdown():
# prepare laser scan
scan.header.stamp = rospy.Time.now()
#self.robot.requestScan()
scan.ranges = self.robot.getScanRanges()
# get motor encoder values
left, right = self.robot.getMotors()
# get analog sensors
self.robot.getAnalogSensors()
# get drop sensors
left_drop = self.robot.state["LeftDropInMM"]
right_drop = self.robot.state["RightDropInMM"]
# send updated movement commands
self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
# ask for the next scan while we finish processing stuff
self.robot.requestScan()
# now update position information
dt = (scan.header.stamp - then).to_sec()
then = scan.header.stamp
d_left = (left - encoders[0])/1000.0
d_right = (right - encoders[1])/1000.0
encoders = [left, right]
dx = (d_left+d_right)/2
dth = (d_right-d_left)/(BASE_WIDTH/1000.0)
x = cos(dth)*dx
y = -sin(dth)*dx
self.x += cos(self.th)*x - sin(self.th)*y
self.y += sin(self.th)*x + cos(self.th)*y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx/dt
odom.twist.twist.angular.z = dth/dt
# prepare drop
drop = NeatoDropSensor()
drop.header.stamp = rospy.Time.now()
drop.left = left_drop
drop.right = right_drop
# publish everything
self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
then, "base_link", "odom" )
self.scanPub.publish(scan)
self.odomPub.publish(odom)
self.dropPub.publish(drop)
# wait, then do it again
r.sleep()
# shut down
self.robot.setLDS("off")
self.robot.setTestMode("off")
def cmdVelCb(self,req):
x = req.linear.x * 1000
th = req.angular.z * (BASE_WIDTH/2)
k = max(abs(x-th),abs(x+th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
self.cmd_vel = [ int(x-th) , int(x+th) ]
class OdometryClass:
def __init__(self):
self.ticks_sub = rospy.Subscriber('/encoders',encoders, self.getTicks)
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=1)
self.odom_broadcaster = TransformBroadcaster()
self.odom = Odometry()
self.rate = rospy.Rate(10)
self.lastLeftTicks = 0
self.lastRightTicks = 0
self.currentLeftTicks = 0
self.currentRightTicks = 0
self.last_time = rospy.Time.now()
self.L = 0.601
self.R = 0.07
self.N = 360
self.x = 0.0
self.y = 0.0
self.th = 0.0
def getTicks(self, msg):
self.currentLeftTicks = msg.encoderTicks[0]
self.currentRightTicks = msg.encoderTicks[1]
def updatePose(self):
while not rospy.is_shutdown():
current_time = rospy.Time.now()
delta_l = self.currentLeftTicks - self.lastLeftTicks
delta_r = self.currentRightTicks - self.lastRightTicks
d_l = 2 * pi * self.R * delta_l / self.N
d_r = 2 * pi * self.R * delta_r / self.N
self.lastLeftTicks = self.currentLeftTicks
self.lastRightTicks = self.currentRightTicks
v = (d_r + d_l) / 2
w = (d_r - d_l) / self.L
# compute odometry in a typical way given the velocities of the robot
dt = (current_time - self.last_time).to_sec()+0.0000000000000000000000000000000000000000001
delta_x = v * cos(self.th)
delta_y = v * sin(self.th)
delta_th = w
self.x += delta_x
self.y += delta_y
self.th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th)
# first, we'll publish the transform over tf
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0.),
odom_quat,
current_time,
"base_link",
"odom"
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(self.x, self.y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(v/dt, 0, 0), Vector3(0, 0, w/dt))
# publish the message
self.odom_pub.publish(odom)
self.last_time = current_time
self.rate.sleep()
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("base_odometry")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param(
'~rate', 10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param(
'ticks_meter',
4900)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param(
'~base_width', 0.5)) # The wheel base width in meters
self.base_frame_id = rospy.get_param(
'~base_frame_id',
'base_link') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param(
'~odom_frame_id',
'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -2147483647)
self.encoder_max = rospy.get_param('encoder_max', 2147483647)
self.encoder_low_wrap = rospy.get_param(
'wheel_low_wrap',
(self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
self.encoder_high_wrap = rospy.get_param(
'wheel_high_wrap',
(self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# subscriptions
rospy.Subscriber("md49_encoders", md49_encoders, self.encodersCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=100)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = (d_left + d_right) / 2
# this approximation works (in radians) for small angles
th = (d_right - d_left) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos(th) * d
y = -sin(th) * d
# calculate the final position of the robot
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
#############################################################################
def encodersCallback(self, data):
#############################################################################
encL = data.encoder_l
if (encL < self.encoder_low_wrap
and self.prev_lencoder > self.encoder_high_wrap):
self.lmult = self.lmult + 1
if (encL > self.encoder_high_wrap
and self.prev_lencoder < self.encoder_low_wrap):
self.lmult = self.lmult - 1
self.left = 1.0 * (encL + self.lmult *
(self.encoder_max - self.encoder_min))
self.prev_lencoder = encL
encR = data.encoder_r
if (encR < self.encoder_low_wrap
and self.prev_rencoder > self.encoder_high_wrap):
self.rmult = self.rmult + 1
if (encR > self.encoder_high_wrap
and self.prev_rencoder < self.encoder_low_wrap):
self.rmult = self.rmult - 1
self.right = 1.0 * (encR + self.rmult *
(self.encoder_max - self.encoder_min))
self.prev_rencoder = encR
def transformCallback(msg):
br = TransformBroadcaster()
br.sendTransform((marker_pose.translation.x, marker_pose.translation.y,
marker_pose.translation.z), (0, 0, 0, 1),
rospy.Time.now(), "fake_force_pose", "world")
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.port = rospy.get_param('~port', "/dev/ttyUSB0")
rospy.loginfo("Using port: %s" % (self.port))
self.robot = xv11(self.port)
rospy.Subscriber("pi_cmd", String, self.pi_command)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('scan', LaserScan)
self.odomPub = rospy.Publisher('odom', Odometry)
self.bumpPub = rospy.Publisher('bump', Bump)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_to_send = None
self.cmd_vel = [0, 0]
def pi_command(self, msg):
self.cmd_to_send = msg
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0
scan.angle_max = 6.26
scan.angle_increment = 0.017437326
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(5)
rospy.sleep(4)
self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
if self.cmd_to_send != None:
self.robot.send_command(self.cmd_to_send)
self.cmd_to_send = None
t_start = time.time()
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
print 'Got scan ranges %f' % (time.time() - t_start)
# get motor encoder values
curr_motor_time = rospy.Time.now()
t_start = time.time()
try:
left, right = self.robot.getMotors()
# now update position information
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
encoders = [left, right]
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (BASE_WIDTH / 1000.0)
total_dth += dth
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
odom.header.stamp = curr_motor_time
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
curr_motor_time, "base_link", "odom")
self.odomPub.publish(odom)
print 'Got motors %f' % (time.time() - t_start)
except:
pass
t_start = time.time()
bump_sensors = self.robot.getDigitalSensors()
# send updated movement commands
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
print 'Set motors %f' % (time.time() - t_start)
# publish everything
self.scanPub.publish(scan)
self.bumpPub.publish(
Bump(leftFront=bump_sensors[0],
leftSide=bump_sensors[1],
rightFront=bump_sensors[2],
rightSide=bump_sensors[3]))
self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
# wait, then do it again
r.sleep()
def cmdVelCb(self, req):
x = req.linear.x * 1000
th = req.angular.z * (BASE_WIDTH / 2)
k = max(abs(x - th), abs(x + th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x * MAX_SPEED / k
th = th * MAX_SPEED / k
self.cmd_vel = [int(x - th), int(x + th)]
print self.cmd_vel, "SENDING THIS VEL"
class NeatoNode(object):
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
host = rospy.get_param('~host')
use_udp = rospy.get_param('~use_udp', True)
rospy.loginfo("Connecting to host: %s" % (host))
self.robot = xv11(host, use_udp)
self.s = rospy.Service('reset_odom', Empty, self.handle_reset_odom)
rospy.Subscriber('cmd_vel', Twist, self.cmdVelCb)
rospy.Subscriber('raw_vel', Float32MultiArray, self.rawVelCb)
self.scanPub = rospy.Publisher('scan', LaserScan, queue_size=10)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.bumpPub = rospy.Publisher('bump', Int8MultiArray, queue_size=10)
self.accelPub = rospy.Publisher('accel',
Float32MultiArray,
queue_size=10)
self.encodersPub = rospy.Publisher('encoders',
Float32MultiArray,
queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = None
self.cmd_vel_lock = threading.Lock()
def handle_reset_odom(self, req):
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
return EmptyResponse()
def spin(self):
old_ranges = None
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = -pi
scan.angle_max = pi
scan.angle_increment = pi / 180.0
scan.range_min = 0.020
scan.range_max = 5.0
scan.time_increment = 1.0 / (5 * 360)
scan.scan_time = 0.2
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(20)
rospy.sleep(4)
# do UDP hole punching to make sure the sensor data from the robot makes it through
self.robot.do_udp_hole_punch()
self.robot.send_keep_alive()
last_keep_alive = time.time()
self.robot.send_keep_alive()
last_keep_alive = time.time()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
last_set_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
if time.time() - last_keep_alive > 30.0:
self.robot.send_keep_alive()
last_keep_alive = time.time()
self.robot.requestScan()
new_stamp = rospy.Time.now()
delta_t = (new_stamp - scan.header.stamp).to_sec()
scan.header.stamp = new_stamp
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
# repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180)
# This is important in order to adhere to ROS conventions regarding laser scanners
if len(scan.ranges):
scan.ranges.append(scan.ranges[0])
scan.intensities.append(scan.intensities[0])
if old_ranges == scan.ranges:
scan.ranges, scan.intensities = [], []
else:
old_ranges = copy(scan.ranges)
if delta_t - 0.2 > 0.1:
print "Iteration took longer than expected (should be 0.2) ", delta_t
# get motor encoder values
curr_motor_time = rospy.Time.now()
try:
motors = self.robot.getMotors()
if motors:
# unpack the motor values since we got them.
left, right = motors
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
# now update position information
# might consider moving curr_motor_time down
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
self.encodersPub.publish(
Float32MultiArray(data=[left / 1000.0, right /
1000.0]))
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
encoders = [left, right]
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (BASE_WIDTH / 1000.0)
total_dth += dth
x_init = self.x
y_init = self.y
th_init = self.th
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
self.th += dth
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
odom.header.stamp = curr_motor_time
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance = [
10**-1, 0, 0, 0, 0, 0, 0, 10**-1, 0, 0, 0, 0, 0, 0,
10**-1, 0, 0, 0, 0, 0, 0, 10**5, 0, 0, 0, 0, 0, 0,
10**5, 0, 0, 0, 0, 0, 0, 10**5
]
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0), (quaternion.x, quaternion.y,
quaternion.z, quaternion.w),
curr_motor_time, "base_link", "odom")
self.odomPub.publish(odom)
except Exception as err:
print "my error is " + str(err)
with self.cmd_vel_lock:
if self.cmd_vel:
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
self.cmd_vel = None
elif rospy.Time.now() - last_set_motor_time > rospy.Duration(
.2):
self.robot.resend_last_motor_command()
last_set_motor_time = rospy.Time.now()
try:
bump_sensors = self.robot.getDigitalSensors()
if bump_sensors:
""" Indices of bump_sensors map as follows
0: front left
1: side left
2: front right
3: side right
"""
self.bumpPub.publish(Int8MultiArray(data=bump_sensors))
except:
print "failed to get bump sensors!"
try:
accelerometer = self.robot.getAccel()
if accelerometer:
# Indices 2, 3, 4 of accelerometer correspond to x, y, and z direction respectively
self.accelPub.publish(
Float32MultiArray(data=accelerometer[2:5]))
except Exception as err:
print "failed to get accelerometer!", err
if len(scan.ranges):
self.scanPub.publish(scan)
# wait, then do it again
r.sleep()
def cmdVelCb(self, req):
# Simple odometry model
x = req.linear.x * 1000
th = req.angular.z * (BASE_WIDTH / 2)
k = max(abs(x - th), abs(x + th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x * MAX_SPEED / k
th = th * MAX_SPEED / k
with self.cmd_vel_lock:
self.cmd_vel = [int(x - th), int(x + th)]
#print self.cmd_vel, "SENDING THIS VEL"
def rawVelCb(self, msg):
if len(msg.data) == 2:
self.cmd_vel = [int(1000.0 * x) for x in msg.data]
class BaseController:
def __init__(self, arduino, base_frame):
self.arduino = arduino
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
self.stopped = False
self.useImu = rospy.get_param("~useImu", False)
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param(
"~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (
self.wheel_diameter * PI)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param(
'wheel_low_wrap',
(self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
self.encoder_high_wrap = rospy.get_param(
'wheel_high_wrap',
(self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
self.l_wheel_mult = 0
self.r_wheel_mult = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# Internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# Subscriptions
#rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
rospy.Subscriber("smoother_cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " +
str(self.wheel_track) + "m wide with " +
str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) +
" Hz using " + str(self.base_frame) + " as base frame")
self.lEncoderPub = rospy.Publisher('Lencoder', Int16, queue_size=5)
self.rEncoderPub = rospy.Publisher('Rencoder', Int16, queue_size=5)
self.lVelPub = rospy.Publisher('Lvel', Int16, queue_size=5)
self.rVelPub = rospy.Publisher('Rvel', Int16, queue_size=5)
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
#rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
self.lEncoderPub.publish(left_enc)
self.rEncoderPub.publish(right_enc)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
if (left_enc < self.encoder_low_wrap
and self.enc_left > self.encoder_high_wrap):
self.l_wheel_mult = self.l_wheel_mult + 1
elif (left_enc > self.encoder_high_wrap
and self.enc_left < self.encoder_low_wrap):
self.l_wheel_mult = self.l_wheel_mult - 1
else:
self.l_wheel_mult = 0
if (right_enc < self.encoder_low_wrap
and self.enc_right > self.encoder_high_wrap):
self.r_wheel_mult = self.r_wheel_mult + 1
elif (right_enc > self.encoder_high_wrap
and self.enc_right < self.encoder_low_wrap):
self.r_wheel_mult = self.r_wheel_mult - 1
else:
self.r_wheel_mult = 0
#dright = (right_enc - self.enc_right) / self.ticks_per_meter
#dleft = (left_enc - self.enc_left) / self.ticks_per_meter
dleft = 1.0 * (left_enc + self.l_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_left) / self.ticks_per_meter
dright = 1.0 * (right_enc + self.r_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_right) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
if (self.useImu == False):
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
# todo sensor_state.distance == 0
#if self.v_des_left == 0 and self.v_des_right == 0:
# odom.pose.covariance = ODOM_POSE_COVARIANCE2
# odom.twist.covariance = ODOM_TWIST_COVARIANCE2
#else:
# odom.pose.covariance = ODOM_POSE_COVARIANCE
# odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)
def cmdVelCallback(self, req):
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter)
self.v_des_right = int(right * self.ticks_per_meter)
class ROSBaseControl:
def __init__(self):
rospy.init_node('base_control')
self.finished = Event()
self.controller = Controller('/dev/ttyS0')
rospy.loginfo("Started base_control")
print "Started base_control"
# odom...
self.rate = float(rospy.get_param("~base_controller_rate", 20))
now = rospy.Time.now()
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = now + self.t_delta
self.then = now # time for determining dx/dy
# internal data
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.vx = 0.0
self.vy = 0.0
self.vth = 0.0
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
def callback_cmd(self, msg):
# TODO: from linear & radial velocities to linear vel on right/left wheel
rospy.loginfo("Received a /cmd_vel message!")
print "Received a /cmd_vel message!"
# from m/s to controller velocity
coef = 58823530
# L = distance between wheels
L = 0.495
if msg.linear.x == 0 and msg.angular.z == 0:
self.controller.stop_motor()
else:
rwheel_vel = (2*msg.linear.x - L*msg.angular.z)/2
lwheel_vel = (2*msg.linear.x + L*msg.angular.z)/2
print rwheel_vel, lwheel_vel
self.controller.set_lwheel_velocity(abs(lwheel_vel*coef))
self.controller.set_rwheel_velocity(abs(rwheel_vel*coef))
self.controller.go_forward()
self.vx = msg.linear.x
#self.vth = msg.angular.z
def listener(self):
rospy.Subscriber('/cmd_vel', Twist, self.callback_cmd)
rosRate = rospy.Rate(self.rate)
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
delta_x = (self.vx * cos(self.th) - self.vy * sin(self.th)) * dt
delta_y = (self.vx * sin(self.th) + self.vy * cos(self.th)) * dt
delta_th = self.vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
class DiffController(Controller):
def __init__(self, node, name):
Controller.__init__(self, node, name)
self.last_cmd = rospy.Time.now()
# Parameters: command timeout
self.timeout = rospy.get_param("~controllers/"+name+"/timeout", 0.5)
# Parameters: geometry
self.ticks_meter = float(rospy.get_param("~controllers/"+name+"/ticks_meter"))
self.base_width = float(rospy.get_param("~controllers/"+name+"/base_width"))
self.base_frame_id = rospy.get_param("~controllers/"+name+"/base_frame_id", "base_link")
self.odom_frame_id = rospy.get_param("~controllers/"+name+"/odom_frame_id", "odom")
# Parameters: PID
self.Kp = rospy.get_param("~controllers/"+name+"/Kp", 1.0)
self.Kd = rospy.get_param("~controllers/"+name+"/Kd", 0.0)
self.Ki = rospy.get_param("~controllers/"+name+"/Ki", 0.1)
self.Kw = rospy.get_param("~controllers/"+name+"/Kw", 400.0)
# Parameters: motor period
self.period = rospy.get_param("~controllers/"+name+"/period", 10.0)
# Parameters: acceleration
self.accel_limit = rospy.get_param("~controllers/"+name+"/accel_limit", 0.1)
# Parameters: twist covariance
self.covariance_vx = rospy.get_param("~controllers/"+name+"/covariance_vx", 1e-3)
self.covariance_vy = rospy.get_param("~controllers/"+name+"/covariance_vy", 1e-3)
self.covariance_vz = rospy.get_param("~controllers/"+name+"/covariance_vz", 1e-6)
self.covariance_wx = rospy.get_param("~controllers/"+name+"/covariance_wx", 1e-6)
self.covariance_wy = rospy.get_param("~controllers/"+name+"/covariance_wy", 1e-6)
self.covariance_wz = rospy.get_param("~controllers/"+name+"/covariance_wz", 1e-3)
# Output for joint states publisher
self.joint_names = ["base_l_wheel_joint", "base_r_wheel_joint"]
self.joint_positions = [0.0, 0.0]
self.joint_velocities = [0.0, 0.0]
# Internal data
self.v_left = 0 # current setpoint velocity
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now() # time for determining dx/dy
# Publish odometry, optionally TF
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
self.odomBroadcaster = None
if rospy.get_param("~controllers/"+name+"/publish_tf", True):
self.odomBroadcaster = TransformBroadcaster()
# Subscribe to command
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
rospy.loginfo("Started DiffController ("+name+").")
rospy.loginfo(" Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
def update(self):
now = rospy.Time.now()
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# Do odometry first
if self.node.sim:
# If simulated, forward simulate trajectory
x = cos(self.th)*self.dx*elapsed
y = -sin(self.th)*self.dx*elapsed
self.x += cos(self.th)*self.dx*elapsed
self.y += sin(self.th)*self.dx*elapsed
self.th += self.dr*elapsed
else:
try:
left = self.node.etherbotix.motor1_pos
right = self.node.etherbotix.motor2_pos
except AttributeError:
# board is not yet updated
return False
# calculate position
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (left - self.enc_left)/self.ticks_meter
d_right = (right - self.enc_right)/self.ticks_meter
self.enc_left = left
self.enc_right = right
d = (d_left+d_right)/2
th = (d_right-d_left)/self.base_width
# calculate velocity
l_vel = self.node.etherbotix.motor1_vel / self.ticks_meter
r_vel = self.node.etherbotix.motor2_vel / self.ticks_meter
self.dx = (l_vel+r_vel)/2 * (1000.0/self.period)
self.dr = (r_vel-l_vel)/self.base_width * (1000.0/self.period)
if (d != 0):
x = cos(th)*d
y = -sin(th)*d
self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
if (th != 0):
self.th = self.th + th
# Update joint_states publisher
self.joint_positions = [self.enc_left/self.ticks_meter, self.enc_right/self.ticks_meter]
self.joint_velocities = [l_vel * (1000.0/self.period), r_vel * (1000.0/self.period)]
# Publish or perish
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th/2)
quaternion.w = cos(self.th/2)
if self.odomBroadcaster:
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
odom.twist.covariance[0] = self.covariance_vx
odom.twist.covariance[7] = self.covariance_vy
odom.twist.covariance[14] = self.covariance_vz
odom.twist.covariance[21] = self.covariance_wx
odom.twist.covariance[28] = self.covariance_wy
odom.twist.covariance[35] = self.covariance_wz
self.odomPub.publish(odom)
# Now update commands
if now > (self.last_cmd + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
# Update motors if real hardware and PID is correct
if not self.node.sim and self.updateParams():
max_accel = int(self.accel_limit * self.ticks_meter * self.dt.to_sec())
# Limit left side acceleration
if self.v_left < self.v_des_left:
self.v_left += max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
# Limit right side acceleration
if self.v_right < self.v_des_right:
self.v_right += max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Send commands
self.updateControls(self.v_left, self.v_right)
## @brief On shutdown, need to stop base.
def shutdown(self):
self.updateControls(0,0)
## @brief ROS callback to set new velocity.
## @param req A geometry_msgs/Twist command.
def cmdVelCb(self, req):
# set motor speeds in ticks per period
self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / (1000.0/self.period))
self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / (1000.0/self.period))
self.last_cmd = rospy.Time.now()
## @brief Get diagnostics message.
def getDiagnostics(self):
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
if not self.node.sim:
msg.values.append(KeyValue("Left", str(self.enc_left)))
msg.values.append(KeyValue("Right", str(self.enc_right)))
msg.values.append(KeyValue("dX", str(self.dx)))
msg.values.append(KeyValue("dR", str(self.dr)))
return msg
## @brief Make sure the parameters on robot match our params.
## @returns True if parameters match.
def updateParams(self):
# Need to check PID params
if not almost_equal(self.node.etherbotix.motor1_kp, self.Kp) or \
not almost_equal(self.node.etherbotix.motor1_kd, self.Kd) or \
not almost_equal(self.node.etherbotix.motor1_ki, self.Ki) or \
not almost_equal(self.node.etherbotix.motor1_windup, self.Kw) or \
not almost_equal(self.node.etherbotix.motor2_kp, self.Kp) or \
not almost_equal(self.node.etherbotix.motor2_kd, self.Kd) or \
not almost_equal(self.node.etherbotix.motor2_ki, self.Ki) or \
not almost_equal(self.node.etherbotix.motor2_windup, self.Kw):
params = struct.pack("<ffff", self.Kp, self.Kd, self.Ki, self.Kw)
params = params + params # both sides are the same
params = [ord(x) for x in params]
self.node.etherbotix.write(253, self.node.etherbotix.P_MOTOR1_KP, params, ret=False)
return False
# Need to check motor period
if self.node.etherbotix.motor_period != self.period:
self.node.etherbotix.write(253, self.node.etherbotix.P_MOTOR_PERIOD, [self.period,] , ret=False)
return False
# Params are up to date
return True
## @brief Update controls
def updateControls(self, left, right):
params = struct.pack("<hh", left, right)
params = [ord(x) for x in params]
self.node.etherbotix.write(253, self.node.etherbotix.P_MOTOR1_VEL, params, ret=False)
class CreateDriver:
def __init__(self):
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
self.create = Create(port)
self.packetPub = rospy.Publisher('sensorPacket',
SensorPacket,
queue_size=10)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.fields = [
'wheeldropCaster', 'wheeldropLeft', 'wheeldropRight', 'bumpLeft',
'bumpRight', 'wall', 'cliffLeft', 'cliffFronLeft',
'cliffFrontRight', 'cliffRight', 'virtualWall', 'infraredByte',
'advance', 'play', 'distance', 'angle', 'chargingState', 'voltage',
'current', 'batteryTemperature', 'batteryCharge',
'batteryCapacity', 'wallSignal', 'cliffLeftSignal',
'cliffFrontLeftSignal', 'cliffFrontRightSignal',
'cliffRightSignal', 'homeBase', 'internalCharger', 'songNumber',
'songPlaying'
]
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
self.create.update = self.sense
def start(self):
self.create.start()
self.then = datetime.now()
def stop(self):
self.create.stop()
def sense(self):
now = datetime.now()
elapsed = now - self.then
self.then = now
elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
d = self.create.d_distance / 1000.
th = self.create.d_angle * pi / 180
dx = d / elapsed
dth = th / elapsed
if (d != 0):
x = cos(th) * d
y = -sin(th) * d
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), "base_link", "odom")
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth
self.odomPub.publish(odom)
packet = SensorPacket()
for field in self.fields:
packet.__setattr__(field, self.create.__getattr__(field))
self.packetPub.publish(packet)
def brake(self, req):
if (req.brake):
self.create.brake()
return BrakeResponse(True)
def circle(self, req):
if (req.clear):
self.create.clear()
self.create.forwardTurn(req.speed, req.radius)
return CircleResponse(True)
def demo(self, req):
self.create.demo(req.demo)
return DemoResponse(True)
def leds(self, req):
self.create.leds(req.advance, req.play, req.color, req.intensity)
return LedsResponse(True)
def tank(self, req):
if (req.clear):
self.create.clear()
self.create.tank(req.left, req.right)
return TankResponse(True)
def turn(self, req):
if (req.clear):
self.create.clear()
self.create.turn(req.turn)
return TurnResponse(True)
def twist(self, req):
x = req.linear.x * 1000.
th = req.angular.z
if (x == 0):
th = th * 180 / pi
speed = (8 * pi * th) / 9
self.create.left(int(speed))
elif (th == 0):
x = int(x)
self.create.tank(x, x)
else:
self.create.forwardTurn(int(x), int(x / th))
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.tf_prefix = rospy.get_param('~tf_prefix', "")
self.port = rospy.get_param('~port', "/dev/ttyUSB0")
rospy.loginfo("Using port: %s"%(self.port))
self.robot = xv11(self.port)
rospy.Subscriber("pi_cmd",String,self.pi_command)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('scan', LaserScan)
self.odomPub = rospy.Publisher('odom',Odometry)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_to_send = None
self.cmd_vel = [0,0]
def pi_command(self,msg):
self.cmd_to_send = msg
def spin(self):
encoders = [0,0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
# things that don't ever change
scan_link = rospy.get_param('~frame_id','base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0
scan.angle_max = 6.26
scan.angle_increment = 0.017437326
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(5)
rospy.sleep(4)
self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
if self.cmd_to_send != None:
self.robot.send_command(self.cmd_to_send)
self.cmd_to_send = None
t_start = time.time()
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
print 'Got scan ranges %f' % (time.time() - t_start)
# get motor encoder values
curr_motor_time = rospy.Time.now()
t_start = time.time()
try:
left, right = self.robot.getMotors()
# now update position information
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
d_left = (left - encoders[0])/1000.0
d_right = (right - encoders[1])/1000.0
encoders = [left, right]
dx = (d_left+d_right)/2
dth = (d_right-d_left)/(BASE_WIDTH/1000.0)
total_dth += dth
x = cos(dth)*dx
y = -sin(dth)*dx
self.x += cos(self.th)*x - sin(self.th)*y
self.y += sin(self.th)*x + cos(self.th)*y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
odom.header.stamp = curr_motor_time
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx/dt
odom.twist.twist.angular.z = dth/dt
self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), curr_motor_time, self.tf_prefix + "/base_link", self.tf_prefix + "/odom" )
self.odomPub.publish(odom)
print 'Got motors %f' % (time.time() - t_start)
except:
pass
t_start = time.time()
# send updated movement commands
self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
print 'Set motors %f' % (time.time() - t_start)
# publish everything
self.scanPub.publish(scan)
self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
# wait, then do it again
r.sleep()
def cmdVelCb(self,req):
x = req.linear.x * 1000
th = req.angular.z * (BASE_WIDTH/2)
k = max(abs(x-th),abs(x+th))
# sending commands higher than max speed will fail
if k > MAX_SPEED:
x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
self.cmd_vel = [ int(x-th) , int(x+th) ]
print self.cmd_vel, "SENDING THIS VEL"
class DiffController(Controller):
""" Controller to handle movement & odometry feedback for a differential
drive mobile base. """
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.pause = True
self.last_cmd = rospy.Time.now()
# parameters: rates and geometry
self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))
self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')
# parameters: PID
self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)
# parameters: acceleration
self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)
# output for joint states publisher
self.joint_names = ["left_wheel_joint","right_wheel_joint"]
self.joint_positions = [0,0]
self.joint_velocities = [0,0]
# internal data
self.v_left = 0 # current setpoint velocity
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now() # time for determining dx/dy
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
def startup(self):
if not self.fake:
self.setup(self.Kp,self.Kd,self.Ki,self.Ko)
def update(self):
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
if self.fake:
x = cos(self.th)*self.dx*elapsed
y = -sin(self.th)*self.dx*elapsed
self.x += cos(self.th)*self.dx*elapsed
self.y += sin(self.th)*self.dx*elapsed
self.th += self.dr*elapsed
else:
# read encoders
try:
left, right = self.status()
except Exception as e:
rospy.logerr("Could not update encoders: " + str(e))
return
rospy.logdebug("Encoders: " + str(left) +","+ str(right))
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (left - self.enc_left)/self.ticks_meter
d_right = (right - self.enc_right)/self.ticks_meter
self.enc_left = left
self.enc_right = right
d = (d_left+d_right)/2
th = (d_right-d_left)/self.base_width
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
x = cos(th)*d
y = -sin(th)*d
self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
if (th != 0):
self.th = self.th + th
# publish or perish
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th/2)
quaternion.w = cos(self.th/2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
if now > (self.last_cmd + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
# update motors
if not self.fake:
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.write(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def shutdown(self):
if not self.fake:
self.write(0,0)
def cmdVelCb(self,req):
""" Handle movement requests. """
self.last_cmd = rospy.Time.now()
if self.fake:
self.dx = req.linear.x # m/s
self.dr = req.angular.z # rad/s
else:
# set motor speeds in ticks per 1/30s
self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
if not self.fake:
msg.values.append(KeyValue("Left", str(self.enc_left)))
msg.values.append(KeyValue("Right", str(self.enc_right)))
msg.values.append(KeyValue("dX", str(self.dx)))
msg.values.append(KeyValue("dR", str(self.dr)))
return msg
###
### Controller Specification:
###
### setup: Kp, Kd, Ki, Ko (all unsigned char)
###
### write: left_speed, right_speed (2-byte signed, ticks per frame)
###
### status: left_enc, right_enc (4-byte signed)
###
def setup(self, kp, kd, ki, ko):
success = self.device.execute(253, AX_CONTROL_SETUP, [10, kp, kd, ki, ko])
def write(self, left, right):
""" Send a closed-loop speed. Base PID loop runs at 30Hz, these values
are therefore in ticks per 1/30 second. """
left = left&0xffff
right = right&0xffff
success = self.device.execute(253, AX_CONTROL_WRITE, [10, left%256, left>>8, right%256, right>>8])
def status(self):
""" read 32-bit (signed) encoder values. """
values = self.device.execute(253, AX_CONTROL_STAT, [10])
left_values = "".join([chr(k) for k in values[0:4] ])
right_values = "".join([chr(k) for k in values[4:] ])
try:
left = unpack('=l',left_values)[0]
right = unpack('=l',right_values)[0]
return [left, right]
except:
return None
class BaseController:
def __init__(self, arduino):
self.arduino = arduino
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param('~base_controller_timeout', 1.0)
self.stopped = False
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Used to Determin which cmd_vel to listen to
self.cmdvel2 = False
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)
def cmdVelCallback(self, req):
#check if we are using cmd_vel2 at the moment
if self.cmdvel2:
return
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
def cmdVel2Callback(self, req):
# Got a request, shut off cmd_vel
self.cmdvel2 = True
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# if we are not turning in place, then go back to cmd_vel
if th == 0:
self.cmdvel2 = False
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
class BaseController:
def __init__(self, arduino, base_frame, name="base_controllers"):
self.arduino = arduino
self.name = name
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 20))
self.timeout = rospy.get_param("~base_controller_timeout", 0.3)
self.accel_limit = rospy.get_param('~accel_limit', 0.05)
self.debugPID = rospy.get_param('~debugPID', False)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
self.linear_scale_correction = rospy.get_param("~linear_scale_correction", 1.0)
self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
self.stopped = False
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 11)
pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)
pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 11)
pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)
pid_params['CWheel_Kp'] = rospy.get_param("~CWheel_Kp", 11)
pid_params['CWheel_Kd'] = rospy.get_param("~CWheel_Kd", 16)
pid_params['CWheel_Ki'] = rospy.get_param("~CWheel_Ki", 0)
pid_params['CWheel_Ko'] = rospy.get_param("~CWheel_Ko", 50)
pid_params['DWheel_Kp'] = rospy.get_param("~DWheel_Kp", 11)
pid_params['DWheel_Kd'] = rospy.get_param("~DWheel_Kd", 16)
pid_params['DWheel_Ki'] = rospy.get_param("~DWheel_Ki", 0)
pid_params['DWheel_Ko'] = rospy.get_param("~DWheel_Ko", 50)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction * 2 / (self.wheel_diameter * pi)
self.ticks_per_meter = self.ticks_per_meter * self.linear_scale_correction
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# Internal data
self.enc_A = None # encoder readings
self.enc_B = None
self.enc_C = None
self.enc_D = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_A = 0
self.v_B = 0
self.v_C = 0
self.v_D = 0
self.v_des_AWheel = 0 # cmd_vel setpoint
self.v_des_BWheel = 0
self.v_des_CWheel = 0
self.v_des_DWheel = 0
self.last_cmd_vel = now
# Subscriptions
rospy.Subscriber("/smooth_cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
# rqt_plot debug pid
if self.debugPID:
self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=10)
self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=10)
self.CEncoderPub = rospy.Publisher('Cencoder', Int32, queue_size=10)
self.DEncoderPub = rospy.Publisher('Dencoder', Int32, queue_size=10)
self.APidoutPub = rospy.Publisher('Apidout', Int32, queue_size=10)
self.BPidoutPub = rospy.Publisher('Bpidout', Int32, queue_size=10)
self.CPidoutPub = rospy.Publisher('Cpidout', Int32, queue_size=10)
self.DPidoutPub = rospy.Publisher('Dpidout', Int32, queue_size=10)
self.AVelPub = rospy.Publisher('Avel', Int32, queue_size=10)
self.BVelPub = rospy.Publisher('Bvel', Int32, queue_size=10)
self.CVelPub = rospy.Publisher('Cvel', Int32, queue_size=10)
self.DVelPub = rospy.Publisher('Dvel', Int32, queue_size=10)
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.wheel_track = self.wheel_track * self.angular_scale_correction
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.AWheel_Kp = pid_params['AWheel_Kp']
self.AWheel_Kd = pid_params['AWheel_Kd']
self.AWheel_Ki = pid_params['AWheel_Ki']
self.AWheel_Ko = pid_params['AWheel_Ko']
self.BWheel_Kp = pid_params['BWheel_Kp']
self.BWheel_Kd = pid_params['BWheel_Kd']
self.BWheel_Ki = pid_params['BWheel_Ki']
self.BWheel_Ko = pid_params['BWheel_Ko']
self.CWheel_Kp = pid_params['CWheel_Kp']
self.CWheel_Kd = pid_params['CWheel_Kd']
self.CWheel_Ki = pid_params['CWheel_Ki']
self.CWheel_Ko = pid_params['CWheel_Ko']
self.DWheel_Kp = pid_params['DWheel_Kp']
self.DWheel_Kd = pid_params['DWheel_Kd']
self.DWheel_Ki = pid_params['DWheel_Ki']
self.DWheel_Ko = pid_params['DWheel_Ko']
self.arduino.update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,self.DWheel_Kp, self.DWheel_Kd, self.DWheel_Ki, self.DWheel_Ko)
def poll(self):
if self.debugPID:
try:
A_pidin, B_pidin, C_pidin, D_pidin = self.arduino.get_pidin()
self.AEncoderPub.publish(A_pidin)
self.BEncoderPub.publish(B_pidin)
self.CEncoderPub.publish(C_pidin)
self.DEncoderPub.publish(D_pidin)
except:
rospy.logerr("getpidin exception count:")
return
try:
A_pidout, B_pidout, C_pidout, D_pidout = self.arduino.get_pidout()
self.APidoutPub.publish(A_pidout)
self.BPidoutPub.publish(B_pidout)
self.CPidoutPub.publish(C_pidout)
self.DPidoutPub.publish(D_pidout)
except:
rospy.logerr("getpidout exception count")
return
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
aWheel_enc, bWheel_enc, cWheel_enc, dWheel_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
#rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_A == None and self.enc_B == None and self.enc_C == None and self.enc_D == None:
d_A = 0
d_B = 0
d_C = 0
d_D = 0
else:
d_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
d_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
d_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter
d_D = (dWheel_enc - self.enc_D) / self.ticks_per_meter
self.enc_A = aWheel_enc
self.enc_B = bWheel_enc
self.enc_C = cWheel_enc
self.enc_D = dWheel_enc
va = d_A/dt;
vb = d_B/dt;
vc = d_C/dt;
vd = d_D/dt;
# forward kinematic
vx = (va + vb + vc + vd) / 4
vy = (-va + vb - vc + vd) / 4
vth = (-va - vb + vc + vd) / (4 * self.wheel_track)
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt;
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame,
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9]
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
odom.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9]
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_AWheel = 0
self.v_des_BWheel = 0
self.v_des_CWheel = 0
self.v_des_DWheel = 0
if self.v_A < self.v_des_AWheel:
self.v_A += self.max_accel
if self.v_A > self.v_des_AWheel:
self.v_A = self.v_des_AWheel
else:
self.v_A -= self.max_accel
if self.v_A < self.v_des_AWheel:
self.v_A = self.v_des_AWheel
if self.v_B < self.v_des_BWheel:
self.v_B += self.max_accel
if self.v_B > self.v_des_BWheel:
self.v_B = self.v_des_BWheel
else:
self.v_B -= self.max_accel
if self.v_B < self.v_des_BWheel:
self.v_B = self.v_des_BWheel
if self.v_C < self.v_des_CWheel:
self.v_C += self.max_accel
if self.v_C > self.v_des_CWheel:
self.v_C = self.v_des_CWheel
else:
self.v_C -= self.max_accel
if self.v_C < self.v_des_CWheel:
self.v_C = self.v_des_CWheel
if self.v_D < self.v_des_DWheel:
self.v_D += self.max_accel
if self.v_D > self.v_des_DWheel:
self.v_D = self.v_des_DWheel
else:
self.v_D -= self.max_accel
if self.v_D < self.v_des_DWheel:
self.v_D = self.v_des_DWheel
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_A, self.v_B, self.v_C, self.v_D)
if self.debugPID:
self.AVelPub.publish(self.v_A)
self.BVelPub.publish(self.v_B)
self.CVelPub.publish(self.v_C)
self.CVelPub.publish(self.v_D)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0, 0, 0)
def cmdVelCallback(self, req):
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
v_x = req.linear.x # m/s
v_y = req.linear.y # m/s
v_th = req.angular.z # rad/s
#reverse kinematic
vA = v_x - v_y - self.wheel_track * v_th
vB = v_x + v_y - self.wheel_track * v_th
vC = v_x - v_y + self.wheel_track * v_th
vD = v_x + v_y + self.wheel_track * v_th
self.v_des_AWheel = int(vA * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_CWheel = int(vC * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_DWheel = int(vD * self.ticks_per_meter / self.arduino.PID_RATE)
class DiffTf:
#############################################################################
#############################################################################
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.245)) # The wheel base width in meters
# emg - base_frame_id should be /robot
# self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
self.base_frame_id = rospy.get_param('~base_frame_id','robot')
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# emg - adding in a holder for the quaternion
self.orientation = Quaternion()
# subscriptions
rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
rospy.Subscriber("Orientation_data", Quaternion, self.orientationCallback)
self.odomPub = rospy.Publisher("odom", Odometry)
# emg - why the tf_broadcaster? is this the /odom -> /robot? if that's
# the case, i thought we'd all agreed that these should be
# explicitly tied together with a static_tf of "0 0 0 0 0 0"
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
#############################################################################
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = ( d_left + d_right ) / 2
# this approximation works (in radians) for small angles
th = ( d_right - d_left ) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos( th ) * d
y = -sin( th ) * d
# calculate the final position of the robot
self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
if( th != 0):
self.th = self.th + th
#->??? Need to check about race condition with callback?????? later.
# it's _possible_ that self.orientation _MIGHT_ change between
# references. if it does, it's hopeful that the dt is very very tiny.
copy_of_orientation = self.orientation
# emg - this is the section that needs to be updated to
# bring in the IMU orientation
# http://answers.ros.org/question/38099/how-to-subscribe-to-a-topic-at-willon-demand/
# http://answers.ros.org/question/37869/subscribing-and-publishing/
# in particular, the second one. it explains using spinOnce to
# block until a message is received and then do other stuff,
# and then go back to the top of a loop or something. which
# is essentially what you've done.
# can have two subscribers - one for the wheels, and one for imu.
# store the imu message in some part of the class and refer to
# it here.
# publish the odom information
# emg quaternion = Quaternion()
# emg quaternion.x = 0.0
# emg quaternion.y = 0.0
# emg quaternion.z = sin( self.th / 2 )
# emg quaternion.w = cos( self.th / 2 )
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
# emg (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
copy_of_orientation, # emg - the self-real orientation from /IMU_data
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
# emg odom.pose.pose.orientation = quaternion
odom.pose.pose.orientation = copy_of_orientation # emg - again
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
#############################################################################
def orientationCallback(self, msg):
# emg - catch a wild quaternion
#############################################################################
self.orientation = msg
#############################################################################
def lwheelCallback(self, msg):
#############################################################################
enc = msg.data
if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap):
self.lmult = self.lmult + 1
if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap):
self.lmult = self.lmult - 1
self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min))
self.prev_lencoder = enc
#############################################################################
def rwheelCallback(self, msg):
#############################################################################
enc = msg.data
if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap):
self.rmult = self.rmult + 1
if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap):
self.rmult = self.rmult - 1
self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min))
self.prev_rencoder = enc
class Robot:
rospy.init_node('omoros', anonymous=True)
# fetch /global parameters
param_port = rospy.get_param('~port')
param_baud = rospy.get_param('~baud')
param_modelName = rospy.get_param('~modelName')
# Open Serial port with parameter settings
ser = serial.Serial(param_port, param_baud)
#ser = serial.Serial('/dev/ttyS0', 115200) #For raspberryPi
ser_io = io.TextIOWrapper(io.BufferedRWPair(ser, ser, 1),
newline = '\r',
line_buffering = True)
config = VehicleConfig()
pose = MyPose()
joyAxes = []
joyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] # Buttons 15
joyDeadband = 0.15
exp = 0.3 # Joystick expo setting
isAutoMode = False
isArrowMode = False # Whether to control robo with arrow key or not
arrowCon = ArrowCon
#initialize data
cmd = Command
enc_L = 0.0 # Left wheel encoder count from QENCOD message
enc_R = 0.0 # Right wheel encoder count from QENCOD message
enc_L_prev = 0.0
enc_R_prev = 0.0
enc_offset_L = 0.0
enc_offset_R = 0.0
enc_cnt = 0
odo_L = 0.0 # Left Wheel odometry returned from QODO message
odo_R = 0.0 # Right Wheel odometry returned from QODO message
RPM_L = 0.0 # Left Wheel RPM returned from QRPM message
RPM_R = 0.0 # Right Wheel RPM returned from QRPM message
speedL = 0.0 # Left Wheel speed returned from QDIFF message
speedR = 0.0 # Reft Wheel speed returned from QDIFF message
vel = 0.0 # Velocity returned from CVW command
rot = 0.0 # Rotational speed returned from CVR command
def __init__(self):
## Set vehicle specific configurations
if self.param_modelName == "r1":
print "**********"
print "Driving R1"
print "**********"
self.config.WIDTH = 0.591 # Apply vehicle width for R1 version
self.config.WHEEL_R = 0.11 # Apply wheel radius for R1 version
self.config.WHEEL_MAXV = 1200.0 # Maximum speed can be applied to each wheel (mm/s)
self.config.V_Limit = 0.6 # Limited speed (m/s)
self.config.W_Limit = 0.1
self.config.V_Limit_JOY = 0.25 # Limited speed for joystick control
self.config.W_Limit_JOY = 0.05
self.config.ArrowFwdStep = 250 # Steps move forward based on Odometry
self.config.ArrowRotRate = 0.125
self.config.encoder.Dir = 1.0
self.config.encoder.PPR = 1000
self.config.encoder.GearRatio = 15
elif self.param_modelName == "mini":
print "***************"
print "Driving R1-mini"
print "***************"
self.config.WIDTH = 0.170 # Apply vehicle width for mini version
self.config.WHEEL_R = 0.0336 # Apply wheel radius for mini version
self.config.WHEEL_MAXV = 500.0
self.config.V_Limit = 0.5
self.config.W_Limit = 0.2
self.config.V_Limit_JOY = 0.5
self.config.W_Limit_JOY = 0.1
self.config.ArrowFwdStep = 100
self.config.ArrowRotRate = 0.1
self.config.encoder.Dir = -1.0
self.config.encoder.PPR = 234
self.config.encoder.GearRatio = 21
else :
print "Error: param:modelName, Only support r1 and mini. exit..."
exit()
print('Wheel Track:{:.2f}m, Radius:{:.2f}m'.format(self.config.WIDTH, self.config.WHEEL_R))
self.config.BodyCircumference = self.config.WIDTH * math.pi
print('Platform Rotation arc length: {:04f}m'.format(self.config.BodyCircumference))
self.config.WheelCircumference = self.config.WHEEL_R * 2 * math.pi
print('Wheel circumference: {:04f}m'.format(self.config.WheelCircumference))
self.config.encoder.Step = self.config.WheelCircumference / (self.config.encoder.PPR * self.config.encoder.GearRatio * 4)
print('Encoder step: {:04f}m/pulse'.format(self.config.encoder.Step))
print(self.ser.name) # Print which port was really used
self.joyAxes = [0,0,0,0,0,0,0,0]
self.joyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
# Configure data output
if self.ser.isOpen():
print("Serial Open")
self.resetODO()
sleep(0.05)
self.reset_odometry()
self.setREGI(0,'QENCOD')
sleep(0.05)
self.setREGI(1,'QODO')
sleep(0.05)
self.setREGI(2,'QDIFFV')
sleep(0.05)
self.setREGI(3,'0')
sleep(0.05)
self.setREGI(4,'0')
#self.setREGI(3,'QVW')
#sleep(0.05)
#self.setREGI(4,'QRPM')
sleep(0.05)
self.setSPERI(20)
sleep(0.05)
self.setPEEN(1)
sleep(0.05)
self.reset_odometry()
# Subscriber
rospy.Subscriber("joy", Joy, self.callbackJoy)
rospy.Subscriber("cmd_vel", Twist, self.callbackCmdVel)
# publisher
self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10)
self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10)
self.pub_motor_status = rospy.Publisher('motor/status', R1MotorStatusLR, queue_size=10)
self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odom_broadcaster = TransformBroadcaster()
rate = rospy.Rate(rospy.get_param('~hz', 30)) # 30hz
rospy.Timer(rospy.Duration(0.05), self.joytimer)
rospy.Timer(rospy.Duration(0.01), self.serReader)
self.pose.timestamp = rospy.Time.now()
while not rospy.is_shutdown():
if self.cmd.isAlive == True:
self.cmd.cnt += 1
if self.cmd.cnt > 1000: #Wait for about 3 seconds
self.cmd.isAlive = False
self.isAutoMode = False
rate.sleep()
self.ser.close()
def serReader(self, event):
reader = self.ser_io.readline()
if reader:
packet = reader.split(",")
try:
header = packet[0].split("#")[1]
if header.startswith('QVW'):
self.vel = int(packet[1])
self.rot = int(packet[2])
elif header.startswith('QENCOD'):
enc_L = int(packet[1])
enc_R = int(packet[2])
if self.enc_cnt == 0:
self.enc_offset_L = enc_L
self.enc_offset_R = enc_R
self.enc_cnt+=1
self.enc_L = enc_L*self.config.encoder.Dir - self.enc_offset_L
self.enc_R = enc_R*self.config.encoder.Dir - self.enc_offset_R
self.pub_enc_l.publish(Float64(data=self.enc_L))
self.pub_enc_r.publish(Float64(data=self.enc_R))
self.pose = self.updatePose(self.pose, self.enc_L, self.enc_R)
#print('Encoder:L{:.2f}, R:{:.2f}'.format(self.enc_L, self.enc_R))
elif header.startswith('QODO'):
self.odo_L = float(packet[1])*self.config.encoder.Dir
self.odo_R = float(packet[2])*self.config.encoder.Dir
#print('Odo:{:.2f}mm,{:.2f}mm'.format(self.odo_L, self.odo_R))
elif header.startswith('QRPM'):
self.RPM_L = int(packet[1])
self.RPM_R = int(packet[2])
#print('RPM:{:.2f}mm,{:.2f}mm'.format(self.RPM_L, self.RPM_R))
elif header.startswith('QDIFFV'):
self.speedL = int(packet[1])
self.speedR = int(packet[2])
except:
pass
status_left = R1MotorStatus(low_voltage = 0, overloaded = 0, power = 0,
encoder = self.enc_L, RPM = self.RPM_L, ODO = self.odo_L, speed = self.speedL)
status_right = R1MotorStatus(low_voltage = 0, overloaded = 0, power = 0,
encoder = self.enc_R, RPM = self.RPM_R, ODO = self.odo_R, speed = self.speedR)
self.pub_motor_status.publish(R1MotorStatusLR(header=Header(stamp=rospy.Time.now()),
Vspeed = self.vel, Vomega = self.rot,
left=status_left, right=status_right))
def callbackJoy(self, data):
self.joyAxes = deepcopy(data.axes)
#print('Joy:{:.2f},{:.2f}'.format(self.joyAxes[0], self.joyAxes[1]))
# Read the most recent button state
newJoyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
newJoyButtons = deepcopy(data.buttons)
# Check if button 1(B) is newly set
if (newJoyButtons[1]==1) and (newJoyButtons[1]!=self.joyButtons[1]):
if self.isAutoMode!= True:
self.isAutoMode = True
print "In Auto mode"
else:
self.isAutoMode = False
print "In Manual mode"
if (newJoyButtons[10]==1) and (newJoyButtons[10]!=self.joyButtons[10]):
if self.isArrowMode!= True:
self.isArrowMode = True
self.arrowCon.isFinished = True
print "Joystick Arrow Mode"
else:
self.isArrowMode = False
print "Joystick Axis Mode"
if self.isArrowMode == True:
#if (newJoyButtons[13]==1) or (newJoyButtons[14]==1):
#if (self.joyAxes[7]==1.0) or (self.joyAxes[7]==-1.0):
if (self.joyAxes[5]==1.0) or (self.joyAxes[5]==-1.0):
if self.arrowCon.isFinished ==True:
self.arrowCon.isFinished = False
#if newJoyButtons[13]==1: # FWD arrow
#if self.joyAxes[7]==1.0:
if self.joyAxes[5]==1.0:
self.arrowCommand("FWD",self.arrowCon,self.config)
else:
self.arrowCommand("REAR",self.arrowCon,self.config)
#print "Arrow: {:.2f} {:.2f} ".format(self.arrowCon.startOdo_L, self.arrowCon.targetOdo_L)
#elif (newJoyButtons[11]==1) or (newJoyButtons[12]==1): #For Xbox360 controller
#elif (self.joyAxes[6]==1.0) or (self.joyAxes[6]==-1.0):
elif (self.joyAxes[4]==1.0) or (self.joyAxes[4]==-1.0):
if self.arrowCon.isFinished ==True:
turnRate = 10.5
self.arrowCon.isFinished = False
#if newJoyButtons[11]==1: # Left arrow
#if self.joyAxes[6]==1.0:
if self.joyAxes[4]==1.0:
self.arrowCommand("LEFT",self.arrowCon, self.config)
else: # Right arrow
self.arrowCommand("RIGHT",self.arrowCon, self.config)
# Update button state
self.joyButtons = deepcopy(newJoyButtons)
def arrowCommand(self, command, arrowCon, config):
if command == "FWD":
arrowCon.setFwd = 1
arrowCon.targetOdo_L = self.odo_L + config.ArrowFwdStep #target 1 step ahead
arrowCon.targetOdo_R = self.odo_R + config.ArrowFwdStep #target 1 step ahead
print "Arrow Fwd"
elif command == "REAR":
self.arrowCon.setFwd = -1
self.arrowCon.targetOdo_L = self.odo_L - self.config.ArrowFwdStep #target 1 step rear
self.arrowCon.targetOdo_R = self.odo_R - self.config.ArrowFwdStep #target 1 step rear
print "Arrow Rev"
elif command == "LEFT":
arrowCon.setRot = 1
arrowCon.targetOdo_L = self.odo_L - config.BodyCircumference*1000*config.ArrowRotRate
arrowCon.targetOdo_R = self.odo_R + config.BodyCircumference*1000*config.ArrowRotRate
print "Arrow Left"
elif command == "RIGHT":
arrowCon.setRot = -1
arrowCon.targetOdo_L = self.odo_L + config.BodyCircumference*1000*config.ArrowRotRate
arrowCon.targetOdo_R = self.odo_R - config.BodyCircumference*1000*config.ArrowRotRate
print "Arrow Right"
def callbackCmdVel(self, cmd):
""" Set wheel speed from cmd message from auto navigation """
if self.isAutoMode:
#print "CMD_VEL: {:.2f} {:.2f} ".format(cmd.linear.x, cmd.angular.z)
cmdV = cmd.linear.x
cmdW = cmd.angular.z
if cmdV>self.config.V_Limit:
cmdV = self.config.V_Limit
elif cmdV<-self.config.V_Limit:
cmdV = -self.config.V_Limit
if cmdW>self.config.W_Limit:
cmdW = self.config.W_Limit
elif cmdW<-self.config.W_Limit:
cmdW = -self.config.W_Limit
(speedL,speedR) = self.getWheelSpeedLR(self.config, cmdV, cmdW)
#print "SPEED LR: {:.2f} {:.2f} ".format(speedL, speedR)
self.sendCDIFFVcontrol(speedL*200, speedR*200)
def reset_odometry(self):
self.last_speedL = 0.0
self.last_speedR = 0.0
def joytimer(self, event):
if not self.isAutoMode:
self.joy_v = self.joyAxes[1]
self.joy_w = self.joyAxes[0]
#print "Joy mode: {:.2f} {:.2f} ".format(self.joy_v, self.joy_w)
else:
return
if not self.isArrowMode:
# Apply joystick deadband and calculate vehicle speed (mm/s) and rate of chage of orientation(rad/s)
joyV = 0.0
joyR = 0.0
if abs(self.joy_v) < self.joyDeadband:
joyV = 0.0
else :
joyV = (1-self.exp) * self.joy_v + (self.exp) * self.joy_v * self.joy_v * self.joy_v
if abs(self.joy_w) < self.joyDeadband:
joyR = 0.0
else :
joyR = (1-self.exp) * self.joy_w + (self.exp) * self.joy_w * self.joy_w * self.joy_w
# Apply max Vehicle speed
(speedL, speedR) = self.getWheelSpeedLR(self.config, joyV * self.config.V_Limit_JOY, joyR * self.config.W_Limit_JOY)
#print "Joystick VL, VR: {:.2f} {:.2f}".format(speedL, speedR)
self.sendCDIFFVcontrol(speedL*1000, speedR*1000)
else:
if self.arrowCon.isFinished == False:
if self.arrowCon.setFwd == 1: # For forward motion
if (self.odo_L < self.arrowCon.targetOdo_L) or (self.odo_R < self.arrowCon.targetOdo_R ):
#print "Fwd: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
self.sendCDIFFVcontrol(100, 100)
else:
self.sendCDIFFVcontrol(0, 0)
self.arrowCon.isFinished = True
self.arrowCon.setFwd = 0
print "Finished!"
elif self.arrowCon.setFwd == -1:
if (self.odo_L > self.arrowCon.targetOdo_L ) or (self.odo_R > self.arrowCon.targetOdo_R ):
#print "Rev: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
self.sendCDIFFVcontrol(-100, -100)
else:
self.sendCDIFFVcontrol(0, 0)
self.arrowCon.isFinished = True
self.arrowCon.setFwd = 0
print "Finished!"
elif self.arrowCon.setRot == 1: #Turning left
if (self.odo_L > self.arrowCon.targetOdo_L) or (self.odo_R < self.arrowCon.targetOdo_R):
#print "Left: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
self.sendCDIFFVcontrol(-100, 100)
else:
self.sendCDIFFVcontrol(0, 0)
self.arrowCon.isFinished = True
self.arrowCon.setRot = 0
print "Finished!"
elif self.arrowCon.setRot == -1: #Turning Right
if (self.odo_L < self.arrowCon.targetOdo_L) or (self.odo_R > self.arrowCon.targetOdo_R):
#print "Right: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
self.sendCDIFFVcontrol(100, -100)
else:
self.sendCDIFFVcontrol(0, 0)
self.arrowCon.isFinished = True
self.arrowCon.setRot = 0
print "Finished!"
def updatePose(self, pose, encoderL, encoderR):
"""Update Position x,y,theta from encoder count L, R
Return new Position x,y,theta"""
now = rospy.Time.now()
dL = encoderL - self.enc_L_prev
dR = encoderR - self.enc_R_prev
self.enc_L_prev = encoderL
self.enc_R_prev = encoderR
dT = (now - pose.timestamp)/1000000.0
pose.timestamp = now
x = pose.x
y = pose.y
theta = pose.theta
R = 0.0
if (dR-dL)==0:
R = 0.0
else:
R = self.config.WIDTH/2.0*(dL+dR)/(dR-dL)
Wdt = (dR - dL) * self.config.encoder.Step / self.config.WIDTH
ICCx = x - R * np.sin(theta)
ICCy = y + R * np.cos(theta)
pose.x = np.cos(Wdt)*(x - ICCx) - np.sin(Wdt)*(y - ICCy) + ICCx
pose.y = np.sin(Wdt)*(x - ICCx) + np.cos(Wdt)*(y - ICCy) + ICCy
pose.theta = theta + Wdt
twist = TwistWithCovariance()
twist.twist.linear.x = self.vel/1000.0
twist.twist.linear.y = 0.0
twist.twist.angular.z = self.rot/1000.0
Vx = twist.twist.linear.x
Vy = twist.twist.linear.y
Vth = twist.twist.angular.z
odom_quat = quaternion_from_euler(0,0,pose.theta)
self.odom_broadcaster.sendTransform((pose.x,pose.y,0.),odom_quat,now,'base_link','odom')
#self.odom_broadcaster.sendTransform((pose.x,pose.y,0.),odom_quat,now,'base_footprint','odom')
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = 'odom'
odom.pose.pose = Pose(Point(pose.x,pose.y,0.),Quaternion(*odom_quat))
odom.child_frame_id = 'base_link'
#odom.child_frame_id = 'base_footprint'
odom.twist.twist = Twist(Vector3(Vx,Vy,0),Vector3(0,0,Vth))
print "x:{:.2f} y:{:.2f} theta:{:.2f}".format(pose.x, pose.y, pose.theta*180/math.pi)
self.odom_pub.publish(odom)
return pose
def getWheelSpeedLR(self, config, V_m_s, W_rad_s):
"""Takes Speed V (m/s) and Rotational sepeed W(rad/s) and compute each wheel speed in m/s
Kinematics reference from http://enesbot.me/kinematic-model-of-a-differential-drive-robot.html"""
speedL = V_m_s - config.WIDTH * W_rad_s / config.WHEEL_R /2.0
speedR = V_m_s + config.WIDTH * W_rad_s / config.WHEEL_R /2.0
return speedL, speedR
def sendCVWcontrol(self, config, V_mm_s, W_mrad_s):
""" Set Vehicle velocity and rotational speed """
if V_mm_s > config.V_Limit :
V_mm_s = config.V_Limit
elif V_mm_s < -config.V_Limit :
V_mm_s = -config.V_limit
if W_mrad_s > config.W_Limit :
W_mrad_s = config.W_Limit
elif W_mrad_s < -config.W_Limit:
W_mrad_s = -config.W_Limit
# Make a serial message to be sent to motor driver unit
cmd = '$CVW,{:.0f},{:.0f}'.format(V_mm_s, W_mrad_s)
print cmd
if self.ser.isOpen():
print cmd
self.ser.write(cmd+"\r"+"\n")
def sendCDIFFVcontrol(self, VLmm_s, VRmm_s):
""" Set differential wheel speed for Left and Right """
if VLmm_s > self.config.WHEEL_MAXV :
VLmm_s = self.config.WHEEL_MAXV
elif VLmm_s < -self.config.WHEEL_MAXV :
VLmm_s = -self.config.WHEEL_MAXV
if VRmm_s > self.config.WHEEL_MAXV :
VRmm_s = self.config.WHEEL_MAXV
elif VRmm_s < -self.config.WHEEL_MAXV :
VRmm_s = -self.config.WHEEL_MAXV
# Make a serial message to be sent to motor driver unit
cmd = '$CDIFFV,{:.0f},{:.0f}'.format(VLmm_s, VRmm_s)
if self.ser.isOpen():
#print cmd
self.ser.write(cmd+"\r"+"\n")
def setREGI(self, param1, param2):
msg = "$SREGI,"+str(param1)+','+param2
self.ser.write(msg+"\r"+"\n")
def setSPERI(self, param):
msg = "$SPERI,"+str(param)
self.ser.write(msg+"\r"+"\n")
def setPEEN(self, param):
msg = "$SPEEN,"+str(param)
self.ser.write(msg+"\r"+"\n")
def resetODO(self):
self.ser.write("$SODO\r\n")
class CreateDriver:
def __init__(self):
port = rospy.get_param('/irobot/serial_port', "/dev/ttyAMA0")
self.autodock = rospy.get_param('/irobot/autodock', 0.0)
self.create = Create(port)
self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
self.odomPub = rospy.Publisher('odom',Odometry)
self.odomBroadcaster = TransformBroadcaster()
self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
self.create.update = self.sense
self.docking = False
def start(self):
self.create.start()
self.then = datetime.now()
def stop(self):
self.create.stop()
def sense(self):
now = datetime.now()
elapsed = now - self.then
self.then = now
elapsed = float(elapsed.seconds) + elapsed.microseconds/1000000.
d = self.create.d_distance / 1000.
th = self.create.d_angle*pi/180
dx = d / elapsed
dth = th / elapsed
if (d != 0):
x = cos(th)*d
y = -sin(th)*d
self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
if (th != 0):
self.th = self.th + th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th/2)
quaternion.w = cos(self.th/2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth
self.odomPub.publish(odom)
packet = SensorPacket()
for field in self.fields:
packet.__setattr__(field,self.create.__getattr__(field))
self.packetPub.publish(packet)
charge_level = float(packet.batteryCharge) / float(packet.batteryCapacity)
if (self.docking and packet.homeBase and charge_level > 0.95):
self.docking = False
self.create.reset()
rospy.sleep(1)
self.create.start()
elif (not self.docking and charge_level < self.autodock):
self.create.demo(1)
self.docking = True
def brake(self,req):
if (req.brake):
self.create.brake()
return BrakeResponse(True)
def circle(self,req):
if (req.clear):
self.create.clear()
self.create.forwardTurn(req.speed,req.radius)
return CircleResponse(True)
def demo(self,req):
self.create.demo(req.demo)
return DemoResponse(True)
def leds(self,req):
self.create.leds(req.advance,req.play,req.color,req.intensity)
return LedsResponse(True)
def tank(self,req):
if (req.clear):
self.create.clear()
self.create.tank(req.left,req.right)
return TankResponse(True)
def turn(self,req):
if (req.clear):
self.create.clear()
self.create.turn(req.turn)
return TurnResponse(True)
def dock(self,req):
self.create.demo(1)
self.docking = True
return DockResponse(True)
def twist(self,req):
x = req.linear.x*1000.
th = req.angular.z
if (x == 0):
th = th*180/pi
speed = (8*pi*th)/9
self.create.left(int(speed))
elif (th == 0):
x = int(x)
self.create.tank(x,x)
else:
self.create.forwardTurn(int(x),int(x/th))