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)
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__(self):
self.ticks_meter = float(100) # The number of wheel encoder ticks per meter of travel
self.base_width = float(0.129) # The wheel base width in meters
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
self.encoder_min = -32768
self.encoder_max = 32768
self.encoder_low_wrap = (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min
self.encoder_high_wrap = (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min
# 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.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = 0
self.odomPub = rospy.Publisher("/odom", Odometry,queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.CMD_RATE =2
self.newTwistCmd = 0
#self.port = rospy.get_param('~port', "/dev/ttyUSB0")
#self.telnet_ip = rospy.get_param('~telnet_ip', "192.168.1.107")
self.telnet_ip = rospy.get_param('~telnet_ip', "Huey-Tiger")
#rospy.loginfo("Using port: %s" % self.port)
rospy.loginfo("Using telnet: %s" % self.telnet_ip)
#self.robot = Botvac(self.port)
#self.robot = Huey(self.telnet_ip)
self.robot = Botvac(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 __init__(self):
print("init")
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
self.create=Create(port)
self.bump=False
self.hz=30
self.rate = 1. / self.hz
self.speed = 200
self.turn = 50
self.distance = 85
self.threshold = .5
self.angle = 18
self.lock = Lock()
self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
self.odomPub=rospy.Publisher('odom', Odometry)
self.odomBroadcaster=TransformBroadcaster()
self.then=datetime.now()
self.x=0
self.y=0
self.th=0
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.create.update = self.sense
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 __init__(self, arduino):
self.arduino = arduino
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param('~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", 4)
pid_params['Ki'] = rospy.get_param("~Ki", 100)
pid_params['Kd'] = rospy.get_param("~Kd", 1)
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
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_mux/input/teleop", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
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 __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 __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 __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 __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()
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)
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 __init__(self):
port = rospy.get_param('/brown/irobot_create/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 __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 __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 __init__(self):
self.moving = False
self.listener = TransformListener()
self.broadcaster = TransformBroadcaster()
self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.face_sub = rospy.Subscriber("face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.faceCallback)
self.lastFaceCallback = time.time()
self.lastHeadTarget = None
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 __init__(self):
super(FakeArlobotNode, self).__init__(name='FakeArlobotNode',
debug=None)
self.last_cmd = rospy.Time.now()
self.rate = 20.0
self.timeout = 3.0
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
self._sub = rospy.Subscriber('/cmd_vel', Twist, self._cmd_vel_callback)
self._speedin_sub = rospy.Subscriber("HALSpeedIn",
HALSpeedIn,
self._speedin_cb,
queue_size=1)
self._positionin_sub = rospy.Subscriber("HALPositionIn",
HALPositionIn,
self._positionin_cb,
queue_size=1)
self._headingin_sub = rospy.Subscriber("HALHeadingIn",
HALHeadingIn,
self._headingin_cb,
queue_size=1)
self.fake = False
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0
self.dr = 0
self.then = rospy.Time.now() # time for determining dx/dy
self.base_frame_id = 'base_footprint'
self.odom_frame_id = 'odom'
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
self._speedout_pub = HALSpeedOutPublisher()
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 __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 = ["base_l_wheel_joint","base_r_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.dy = 0 # add by yang
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 yang DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
def __init__(self,port):
self.create = create.Create(port,3)
self.create.playSong(((70,8),(72,8),(68,8),(56,8),(63,8)))
self.packetPub = rospy.Publisher('~sensorData', 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.dist = 0
self.theta = 0
self.x = 0
self.y = 0
self.last = rospy.Time.now()
self.baseFrame = rospy.get_param("~base_frame","/base_link")
self.odomFrame = rospy.get_param("~odom_frame","/odom")
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
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.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = [0,0]
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 main(self):
self.odomPub = rospy.Publisher('odometry/filtered',
Odometry,
queue_size=10)
self.tfPub = TransformBroadcaster()
rospy.init_node('odometry_node')
self.nodeName = rospy.get_name()
rospy.loginfo("{0} started".format(self.nodeName))
self.ticksPerMeter = int(rospy.get_param('~ticks_per_meter', 780))
self.wheelSeparation = float(rospy.get_param('~wheel_separation', 0.7))
self.rate = float(rospy.get_param('~rate', 10.0))
self.baseFrameID = rospy.get_param('~base_frame_id', 'base_footprint')
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.setTime(rospy.get_time())
rate = rospy.Rate(self.rate)
rospy.Subscriber("encoder_counts",
EncoderCounts,
callback=self.callback)
rospy.spin()
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 __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
# TODO: Change ticks meter
# self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
# TODO: Modify the base width
self.base_width = float(rospy.get_param('~base_width', 0.465)) # 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', -128)
# self.encoder_max = rospy.get_param('encoder_max', 128)
# 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
# Originally, the subscription type was Int16
# Changed to Float32 to allow for more precise driving
rospy.Subscriber("lwheel", Float32, self.lwheelCallback)
rospy.Subscriber("rwheel", Float32, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
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.meter = float(rospy.get_param('meter', 0.6094)) # The number of meters per wheel rotation
self.ticks = float(rospy.get_param('ticks', 2000)) # The number of wheel encoder ticks per wheel rotation
self.ticks_meter = float(self.ticks / self.meter)
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.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("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry)
self.odomBroadcaster = TransformBroadcaster()
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()
def __init__(self):
# Give the node a name
rospy.init_node('odom_ekf', anonymous=False)
# Publisher of type nav_msgs/Odometry
self.ekf_pub = rospy.Publisher('odom', Odometry, queue_size=5)
# Wait for the /odom_combined topic to become available
rospy.wait_for_message('robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped)
rospy.wait_for_message('encoder', Encoder)
# Subscribe to the /odom_combined topic
rospy.Subscriber('robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped, self.pub_ekf_odom)
rospy.Subscriber('encoder', Encoder, self.pub_ekf_twist)
self.odom = Odometry()
self.odom_in = Odometry()
self.odom_broadcaster = TransformBroadcaster()
self.twist = Twist()
rate = rospy.Rate(20)
while not rospy.is_shutdown():
# self.odom_broadcaster.sendTransform((self.odom_in.pose.pose.position.x, self.odom_in.pose.pose.position.y, 0),
# (self.odom_in.pose.pose.orientation.x, self.odom_in.pose.pose.orientation.y, self.odom_in.pose.pose.orientation.z, self.odom_in.pose.pose.orientation.w)
# , rospy.Time.now(), "base_link", "odom")
self.odom.header = self.odom_in.header
self.odom.child_frame_id = "base_link"
#self.odom.header.stamp =rospy.Time.now()
self.odom.pose = self.odom_in.pose
self.odom.twist.twist = self.twist
'''
if self.odom.twist.twist.linear.x == 0 and self.odom.twist.twist.angular.z == 0:
self.odom.pose.covariance=ODOM_POSE_COVARIANCE2
self.odom.twist.covariance=ODOM_POSE_COVARIANCE2
else:
self.odom.pose.covariance=ODOM_POSE_COVARIANCE
self.odom.twist.covariance=ODOM_POSE_COVARIANCE
'''
self.ekf_pub.publish(self.odom)
rate.sleep()
rospy.spin()
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', 469)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.3)) # 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 was base_link
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()
# subscriptions
rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
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.3)) # 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.then = rospy.Time.now()
# subscriptions
rospy.Subscriber("lwheel", Int64, self.lwheelCallback) #Output from the left and right wheel encoders. The hardware is expected to count pulses from the wheel encoders, and provide the total number of pulses since the start of tracking.
rospy.Subscriber("rwheel", Int64, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10) #Publishes the odometry (the current pose and twist) of the robot.
self.odomBroadcaster = TransformBroadcaster()
def __init__(self, base_frame, wheel_diameter, wheel_spread,
encoder_resolution, gear_reduction, ticks_per_meter):
""" *Dead_Reckon*: Initialize an *Odometry* object to contain
*base_frame*,
"""
# Verify argument types:
assert isinstance(base_frame, str)
assert isinstance(encoder_resolution, int)
assert isinstance(gear_reduction, float)
assert isinstance(ticks_per_meter, float)
assert isinstance(wheel_diameter, float)
assert isinstance(wheel_spread, float)
# Get the starting time *now*:
now = rospy.Time.now()
# Load up *self*:
self.base_frame_ = base_frame
self.covariance_ = [
0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2
]
self.encoder_resolution_ = encoder_resolution
self.gear_reduction_ = gear_reduction
self.left_encoder_ = 0
self.right_encoder_ = 0
self.now_ = now
self.odom_ = Odometry()
self.odom_pub_ = rospy.Publisher('odom', Odometry, queue_size=5)
self.odom_broadcaster_ = TransformBroadcaster()
self.previous_left_encoder_ = 0
self.previous_now_ = now
self.previous_right_encoder_ = 0
self.quaternion_ = Quaternion()
self.th_ = 0.0 # Theta is the robot bearing
self.ticks_per_meter_ = ticks_per_meter
self.wheel_diameter_ = wheel_diameter
self.wheel_spread_ = wheel_spread
self.x_ = 0.0
self.y_ = 0.0
def __init__(self):
rospy.init_node("wheel_odometry")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started"% self.nodename)
#PARAMETERS
self.rate= rospy.get_param('~rate',10.0) #Rate at which to publish
self.ticks_meter = float(rospy.get_param('ticks_meter',)) #No. of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base width',))#wheel base width
self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') #name of base frame of robot
self.odom_frame_id = rospy.get_param('~odom_frame_id','odom') #name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min',)
self.encoder_max = rospy.get_param('encoder_max',)
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
#data
self.enc_left = None
self.enc_right = None
self.left = 0
self.right = 0
self.lmult =0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0
self.y = 0
self.theta= 0
self.dx = 0
self.dr = 0
self.then = rospy.Time.now()
#subscribed topics
rospy.Subscriber("lwheel",Int64,self.lwheelCallback)
rospy.Subscriber("rwheel",Int64,self.rwheelCallback)
self.odomPub = rospy.Publisher("odom",Odometry,queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
def __init__(self):
# If /create/use_host_name is True, the topic names used by
# this driver will be /hostname/cmd_vel, /hostname/odom, etc.
self.hn = HName(usename = rospy.get_param('/create/use_host_name', False))
port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
self.create = Create(port)
self.packetPub = rospy.Publisher(self.hn.topic('sensorPacket'), SensorPacket)
self.odomPub = rospy.Publisher(self.hn.topic('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.idealTh = 0
self.create.update = self.sense
# SW
self.create.storeSong(2,67,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,72,11,69,32)
# CE3K
self.create.storeSong(3,74,16,81,24,83,24,79,24,67,32,74,40)
# HN
self.create.storeSong(4,74,64,78,20,75,20,74,20,78,56,81,17,79,17,78,17,79,48,82,14,81,14,79,14,78,28,75,32,74,40)
# Js
self.create.storeSong(5,42,32,43,32,42,32,43,32,42,32,43,32,42,32,43,32)
# ROLA
self.create.storeSong(6,64,24,65,8,67,16,72,40,62,24,64,8,65,48,67,24,69,8,71,16,77,40,69,24,71,10,72,24,74,24,76,48)
# JJMD
self.create.storeSong(7,79,12,81,12,83,12,86,12,84,12,84,12,88,12,86,12,86,12,91,12,90,12,91,12,86,12,83,12,79,40)
# SaaHC
self.create.storeSong(8,84,25,79,18,79,12,81,25,79,40,83,28,84,12)
# Angry men
self.create.storeSong(9,64,24,62,12,60,24,62,12,64,24,65,12,67,36,64,12,62,12,60,12,59,24,57,12,59,24,60,12,55,48)
# IGR
self.create.storeSong(10,77,24,79,24,82,24,84,40)
#,84,24,82,24,79,24,77,40,77,24,79,24,82,24,84,18,87,18,84,10,86,32,84,42)
self.create.storeSong(11,50,10,51,10)
self.create.storeSong(12,51,10,50,10)
self.create.storeSong(13,52,10,51,10,50,10)
self.create.storeSong(14,53,10,54,10,55,10)
self.create.storeSong(15,54,10,54,10)
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 __init__(self):
port = rospy.get_param("/brown/irobot_create_2_1/port", "/dev/ttyUSB0")
self.roomba = Roomba(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",
"cliffFrontLeft",
"cliffFrontRight",
"cliffRight",
"virtualWall",
"overCurrentLeft",
"overCurrentRight",
"overCurrentMainBrush",
"overCurrentSideBrush",
"dirtDetectorLeft",
"dirtDetectorRight",
"virtualWall",
"infraredByte",
"button",
"distance",
"angle",
"chargingState",
"voltage",
"current",
"batteryTemperature",
"batteryCharge",
"batteryCapacity",
]
self.then = datetime.now()
self.x = 0
self.y = 0
self.th = 0
self.roomba.update = self.sense
def __init__(self, arduino):
self.arduino = arduino
self.stopped = False
self.forwardSpeed = 0
self.angularSpeed = 0
# # Subscribe to bupigo_speeds
# rospy.Subscriber("bupigo_wheel_speeds", WheelSpeeds, self.bupigoSetSpeedsCallback)
# Subscribe to cmd_vel
rospy.Subscriber("bupigo_cmd_vel", Twist, self.bupigoCmdVelCallback)
# Subscribe to bupigo_servo_angle
rospy.Subscriber("bupigo_servo_angle", UInt8, self.bupigoSetServoCallback)
# Setup blob publisher
self.blob_publisher = rospy.Publisher('bupigo_blobs', Blobs, queue_size=1)
# Setup the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1)
self.odomBroadcaster = TransformBroadcaster()
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()
def __init__(self):
self.br = None
self.server = None
self.rate_config = rospy.get_param('~rate', 10.0)
self.current_waypoint_position_x = rospy.get_param(
'~current_waypoint_position_x', 0.0)
self.current_waypoint_position_y = rospy.get_param(
'~current_waypoint_position_y', 0.0)
self.current_waypoint_position_z = rospy.get_param(
'~current_waypoint_position_z', 0.0)
self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
# mavros marker setup
# configure waypoint publisher
self.mavros_waypoint_pub = rospy.Publisher("/mavros/mission/waypoints",
Waypoint,
queue_size=1)
self.waypoint_msg = WaypointList()
# define rate
self.rate = rospy.Rate(self.rate_config)
self.br = TransformBroadcaster()
self.server = InteractiveMarkerServer("waypoint_interactive_publisher")
position = Point(self.current_waypoint_position_x,
self.current_waypoint_position_y, 0)
self.make_auv_waypoint_Marker(position)
self.server.applyChanges()
# infinity loop
while not rospy.is_shutdown():
self.update_waypoints()
self.rate.sleep()
def forward_DH(self, q=None, start=1, end=None):
if q is None:
q = self.q
if end is None:
end = len(self.q)
q[4] = 0.4
H = transforms.identity_matrix()
for i in range(start, end):
Rz = transforms.rotation_matrix(self.DH[i][3] + q[i], (0, 0, 1))
Tz = transforms.translation_matrix((0, 0, self.DH[i][2]))
Tx = transforms.translation_matrix((self.DH[i][1], 0, 0))
Rx = transforms.rotation_matrix(self.DH[i][0], (1, 0, 0))
A = transforms.concatenate_matrices(Rz, Tz, Tx, Rx)
print(A)
H = transforms.concatenate_matrices(H, A)
#out = "%f\t%f\t%f\t%f + %f" % (self.DH[i][1], self.DH[i][0],
#self.DH[i][2], self.q[i], self.DH[i][3])
#rospy.logdebug(out)
xyz = H[:3, 3]
qtn = transforms.quaternion_from_matrix(H)
rpy = transforms.euler_from_matrix(H[:3, :3], axes='sxyz')
print(H)
# check #################
br = TransformBroadcaster()
ls = tf.TransformListener()
try:
ls.waitForTransform("tool0", "iiwa_base_link", rospy.Time(0),
rospy.Duration(4.0))
(pos, rot) = ls.lookupTransform('iiwa_base_link', 'tool0',
rospy.Time())
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("can't get iiwa_base_link <- tool0 transform")
return -1
print(pos)
##########################
return xyz, qtn, rpy, H
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)
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)
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)
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")
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 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"
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 : 20 ticks (resolution of encoder disk)
# For 1 meter: 20 * ( 1 / 0.2041) = 98 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',
98)) # 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)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
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 main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('collect_motion_data')
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
parser = optparse.OptionParser()
parser.add_option('-s',
action="store",
dest='shape_id',
help='The shape id e.g. rect1-rect3',
default='rect1')
parser.add_option('',
'--surface',
action="store",
dest='surface_id',
help='The surface id e.g. plywood, abs',
default='plywood')
parser.add_option('-r',
'--real',
action="store_true",
dest='real_exp',
help='Do the real experiment space',
default=False)
parser.add_option('',
'--slow',
action="store_true",
dest='slow',
help='Set slower global speed',
default=False)
(opt, args) = parser.parse_args()
# set the parameters
global globalvel
global global_slow_vel
globalvel = 300 # speed for moving around
globalmaxacc = 100 # some big number means no limit, in m/s^2
globalacc = 1 # some big number means no limit, in m/s^2
global_slow_vel = 30
if opt.slow: globalvel = global_slow_vel
ori = [0, 0, 1, 0]
probe_id = 'probe4'
probe_lengths = {
'probe1': 0.23746,
'probe2': 0.16649,
'probe3': 0.15947,
'probe4': 0.15653
}
probe_length = probe_lengths[
probe_id] # probe1: 0.00626/2 probe2: 0.004745 probe3: 0.00475
ft_length = 0.04703
z = probe_length + ft_length + 0.004 + 0.00 # the height above the table
# parameters about the surface
surface_id = opt.surface_id
surface_thicks = {
'plywood': 0.01158,
'abs': 0.01436,
'silicone_rubber': 0.01436
}
surface_thick = surface_thicks[surface_id] # 0.01158 for plywood
z = z + surface_thick
z_recover = 0.012 + z # the height for recovery probe2: 0.2265 probe 3: 0.2226
zup = z + 0.08 + 0.1 # the prepare and end height
probe_radii = {
'probe1': 0.00626 / 2,
'probe2': 0.004745,
'probe3': 0.00475
}
probe_radius = probe_radii[probe_id]
dist_before_contact = 0.03
dist_after_contact = 0.05
skip_when_exists = True
reset_freq = 1
global_frame_id = '/map'
# parameters about object
shape_id = opt.shape_id
shape_db = ShapeDB()
shape_type = shape_db.shape_db[shape_id]['shape_type']
shape = shape_db.shape_db[shape_id]['shape']
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
# space for the experiment
real_exp = opt.real_exp
if real_exp:
#speeds = reversed([20, 50, 100, 200, 400])
speeds = reversed([-1, 20, 50, 100, 200, 400])
#speeds = reversed([20])
if shape_type == 'poly':
side_params = np.linspace(0, 1, 11)
else:
side_params = np.linspace(0, 1, 40, endpoint=False)
angles = np.linspace(-pi / 180.0 * 80.0, pi / 180 * 80, 9)
else:
speeds = [20, 100, 400, -1]
#speeds = reversed([-1])
#shape = [shape[0]]
side_params = [0.1] #np.linspace(0.1,0.9,3)
angles = [0] #np.linspace(-pi/4, pi/4, 3)
# parameters about rosbag
dir_save_bagfile = os.environ[
'PNPUSHDATA_BASE'] + '/straight_push/%s/push_dataset_motion_full_%s/' % (
surface_id, shape_id)
#topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker']
topics = ['-a']
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
make_sure_path_exists(dir_save_bagfile)
# hack
limit = 100
cnt = 0
# enumerate the speed
for v in speeds:
# enumerate the side we want to push
for i in range(len(shape)):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
#pos = np.array(shape[i]) *s + np.array(shape[(i+1) % len(shape)]) *(1-s)
pos = np.array(shape[i]) * (1 - s) + np.array(shape[
(i + 1) % len(shape)]) * (s)
pos = np.append(pos, [0])
tangent = np.array(shape[(i + 1) % len(shape)]) - np.array(
shape[i])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
elif shape_type == 'ellip':
(a, b) = shape[0][0], shape[0][1]
pos = [
shape[0][0] * np.cos(s * 2 * np.pi),
shape[0][1] * np.sin(s * 2 * np.pi), 0
]
normal = [
np.cos(s * 2 * np.pi) / a,
np.sin(s * 2 * np.pi) / b, 0
]
normal = normal / norm(normal) # normalize it
elif shape_type == 'polyapprox':
pos, normal = polyapprox(shape, s)
# enumerate the direction in which we want to push
for t in angles:
bagfilename = 'motion_surface=%s_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (
surface_id, shape_id, v, i, s, t)
bagfilepath = dir_save_bagfile + bagfilename
# if exists then skip it
if skip_when_exists and os.path.isfile(bagfilepath):
print bagfilepath, 'exits', 'skip'
continue
# find the probe pos in contact in object frame
pos_probe_contact_object = pos + normal * probe_radius
# find the start point
direc = np.dot(tfm.euler_matrix(0, 0, t),
normal.tolist() +
[1])[0:3] # in the direction of moving out
pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact
if shape_type == 'polyapprox' and polyapprox_check_collision(
shape, pos_start_probe_object, probe_radius):
print bagfilename, 'will be in collision', 'skip'
continue
# find the end point
pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact
# zero force torque sensor
rospy.sleep(0.1)
setZero()
wait_for_ft_calib()
# transform start and end to world frame
pos_start_probe_world = coordinateFrameTransform(
pos_start_probe_object, obj_frame_id, global_frame_id,
listener)
pos_end_probe_world = coordinateFrameTransform(
pos_end_probe_object, obj_frame_id, global_frame_id,
listener)
pos_contact_probe_world = coordinateFrameTransform(
pos_probe_contact_object, obj_frame_id,
global_frame_id, listener)
# start bag recording
# move to startPos
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos, ori)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos, ori)
rosbag_proc = subprocess.Popen(
'rosbag record -q -O %s %s' %
(bagfilename, " ".join(topics)),
shell=True,
cwd=dir_save_bagfile)
print 'rosbag_proc.pid=', rosbag_proc.pid
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if v > 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=v, ori=1000)
setCart(end_pos, ori)
setSpeed(tcp=globalvel, ori=1000)
setAcc(acc=globalacc, deacc=globalacc)
else: # v < 0 acceleration
setSpeed(tcp=30, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
setCart(mid_pos, ori)
setAcc(acc=-v, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
setCart(end_pos, ori)
setSpeed(tcp=globalvel, ori=1000)
setAcc(acc=globalacc, deacc=globalacc)
# end bag recording
terminate_ros_node("/record")
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos, ori)
# recover
recover(obj_frame_id, global_frame_id, z_recover, obj_slot,
not (cnt % reset_freq))
#pause()
cnt += 1
if cnt > limit:
break
if cnt > limit:
break
if cnt > limit:
break
if cnt > limit:
break
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 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()
x4 = (0., 0., 0.)
psi = s[2][0]
phi = s[3][0]
# print "sending transform"
q1 = tf.transformations.quaternion_from_euler(0., 0., 0., 'sxyz')
# q4 = tf.transformations.quaternion_from_euler(s[6],s[7],s[8],'sxyz')
#if psi == 0:
# psi = 0.001
pub_h.publish(s[4][0])
pub_va.publish(s[6][0])
pub_phi.publish(s[3][0])
q4 = tf.transformations.quaternion_from_euler(phi, 0., psi, 'sxyz')
# print psi, "quat", q1,q4
br.sendTransform(x1, q1, time, "veh", "world")
br.sendTransform(x4, q4, time, "base_link", "veh")
time_old = time.to_sec()
# rospy.sleep(0.1)
if __name__ == '__main__':
rospy.init_node('new_autopilot_phi')
br = TransformBroadcaster()
try:
start_funct()
except:
rospy.ROSInterruptException
pass
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")
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()
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
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 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]