class FakeArlobotNode(BaseNode):
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 _speedin_cb(self, msg):
if not self.fake:
self.dx = msg.linear
self.dr = msg.angular
def _positionin_cb(self, msg):
if not self.fake:
self.x = msg.x
self.y = msg.y
def _headingin_cb(self, msg):
if not self.fake:
self.th = msg.heading
def _cmd_vel_callback(self, msg):
self.last_cmd = rospy.Time.now()
self.dx = msg.linear.x # m/s
self.dr = msg.angular.z # rad/s
self._speedout_pub.publish(SpeedData(linear=self.dx, angular=self.dr))
def start(self):
if self.fake:
self._speedout_pub.publish(SpeedData(linear=0.0, angular=0.0))
def loop(self):
while not rospy.is_shutdown():
now = rospy.Time.now()
if now > self.t_next:
if self.fake:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
x = cos(self.th) * self.dx * elapsed
y = -sin(self.th) * self.dx * elapsed
self.x += cos(self.th) * self.dx * elapsed
self.y += sin(self.th) * self.dx * elapsed
self.th += self.dr * elapsed
# Update odometry and broadcast
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
def shutdown(self):
if self.fake:
self._speedout_pub.publish(SpeedData(linear=0.0, angular=0.0))
class PubTestNode(BaseNode):
"""
Hardware Abstraction Layer (HAL) node. Exposes messages for sending and receiving data from
hardware devices.
"""
def __init__(self, debug=False):
super(PubTestNode, self).__init__(name='PubTestNode', debug=debug)
rospy.loginfo("PubTestNode init")
self._rate = rospy.Rate(1)
self._statusin_pub = HALStatusInPublisher()
self._speedin_pub = HALSpeedInPublisher()
self._posin_pub = HALPositionInPublisher()
self._headingin_pub = HALHeadingInPublisher()
self._heartbeatin_pub = HALHeartbeatInPublisher()
self._speedout_pub = HALSpeedOutPublisher()
self._controlout_pub = HALControlOutPublisher()
self._orientin_pub = HALOrientationInPublisher()
self._accelin_pub = HALLinearAccelInPublisher()
self._angvelin_pub = HALAngularVelocityInPublisher()
self._magneticin_pub = HALMagneticInPublisher()
self._eulerin_pub = HALEulerInPublisher()
self._tempin_pub = HALTempInPublisher()
self._sensorin_pub = HALSensorArrayInPublisher()
def start(self):
pass
def loop(self):
while not rospy.is_shutdown():
self._statusin_pub.publish(
hwmsg.StatusData(device=0x5555, calibration=0x3333))
self._speedin_pub.publish(
hwmsg.SpeedData(left=3.1415, right=3.1415))
self._posin_pub.publish(hwmsg.PositionData(x=3.1415, y=3.1415))
self._headingin_pub.publish(hwmsg.HeadingData(heading=3.1415))
self._heartbeatin_pub.publish(hwmsg.HeartbeatData(heartbeat=12345))
self._speedout_pub.publish(
hwmsg.SpeedData(left=3.1415, right=3.1415))
self._controlout_pub.publish(
hwmsg.ControlData(device=0xAAAA, debug=0xCCCC))
self._orientin_pub.publish(
hwmsg.OrientationData(x=3.1415, y=3.1415, z=3.1415, w=3.1415))
self._accelin_pub.publish(
hwmsg.LinearAccelData(x=3.1415, y=3.1415, z=3.1415))
self._angvelin_pub.publish(
hwmsg.AngularVelocityData(x=3.1415, y=3.1415, z=3.1415))
self._magneticin_pub.publish(
hwmsg.MagneticData(x=3.1415, y=3.1415, z=3.1415))
self._eulerin_pub.publish(
hwmsg.EulerData(yaw=3.1415, pitch=3.1415, roll=3.1415))
self._tempin_pub.publish(hwmsg.TemperatureData(f=3.1415, c=3.1415))
self._sensorin_pub.publish(
hwmsg.SensorData(source='infrared', data=[3.1415] * 16))
self._sensorin_pub.publish(
hwmsg.SensorData(source='ultrasonic', data=[3.1415] * 16))
self._rate.sleep()
def shutdown(self):
pass
class DriveNode(BaseNode):
"""
The DriveNode is responsible for:
* accepting Twist messages to be sent to the motor driver.
* ensuring max linear/angular velocity and applying acceleration limits.
* reading speed, distance and heading data from the hardware drivers, calculating and publishing
odometry.
"""
def __init__(self, debug=False):
super(DriveNode, self).__init__(name='DriveNode', debug=debug)
"""
------------------------------------------------------------------------------------------
Publishers
------------------------------------------------------------------------------------------
"""
# Publisher of speed to motor driver and odometry to other nodes
self._speedout_pub = HALSpeedOutPublisher()
self._odom_pub = OdometryPublisher()
"""
------------------------------------------------------------------------------------------
Parameters
------------------------------------------------------------------------------------------
"""
# Robot physical dimensions
self._track_width = rospy.get_param('Track Width', 0.3937)
self._wheel_radius = rospy.get_param('Wheel Radius', 0.0785)
# Main loop rate in hz
drive_node_rate = rospy.get_param('Drive Node Rate', 10)
self._loop_rate = rospy.Rate(drive_node_rate)
self._loop_period = 1.0 / float(drive_node_rate)
max_wheel_rpm = rospy.get_param('Max Wheel RPM', 95.0)
max_wheel_ang_vel = rpm_to_rps(max_wheel_rpm, self._wheel_radius)
# Calculate max unicycle velocities based on wheel maximum, track width and wheel radius
max_lin_vel, max_ang_vel = uni_max(max_wheel_ang_vel,
self._track_width,
self._wheel_radius)
max_angular_accel = rospy.get_param('Max Angular Accel', 0.15)
max_angular_decel = rospy.get_param('Max Angular Decel', 0.15)
self._angular_max = AngularMax(ccw=max_ang_vel,
cw=-max_ang_vel,
accel=max_angular_accel,
decel=-max_angular_decel,
wheel=max_ang_vel)
max_linear_accel = rospy.get_param('Max Linear Accel', 0.035)
max_linear_decel = rospy.get_param('Max Linear Decel', 0.035)
self._linear_max = LinearMax(forward=max_lin_vel,
backward=-max_lin_vel,
accel=max_linear_accel,
decel=-max_linear_decel)
# PID controllers
linear_kp = rospy.get_param('Linear Gain Kp', 0.8)
linear_ki = rospy.get_param('Linear Gain Ki', 0.0)
linear_kd = rospy.get_param('Linear Gain Kd', 0.0)
self._lin_pid = PID('linear', linear_kp, linear_ki, linear_kd, 0.0,
max_lin_vel, self._loop_period)
angular_kp = rospy.get_param('Angular Gain Kp', 0.8)
angular_ki = rospy.get_param('Angular Gain Ki', 0.0)
angular_kd = rospy.get_param('Angular Gain Kd', 0.0)
self._ang_pid = PID('angular', angular_kp, angular_ki, angular_kd, 0.0,
max_ang_vel, self._loop_period)
# State variable storage
self._vel_state = VelocityState()
self._odometry = OdometryState()
self._cmd_vel_state = CommandVelocityState()
rospy.loginfo(
STATUS_FMT.format(self._track_width, self._wheel_radius,
self._wheel_radius * 2,
self._wheel_radius * 2 * math.pi,
max_wheel_ang_vel, max_ang_vel, max_lin_vel,
max_angular_accel, max_linear_accel))
"""
-------------------------------------------------------------------------------------------
Subscribers
-------------------------------------------------------------------------------------------
"""
# Note: Declare subscribers at the end to prevent premature callbacks
# Subscriber for Twist messages
self._twist_sub = rospy.Subscriber("cmd_vel",
Twist,
self._twist_cmd_cb,
queue_size=1)
# Subscriber for motor driver speed, distance, and orientation messages
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._eulerin_sub = rospy.Subscriber("HALEulerIn",
HALEulerIn,
self._eulerin_cb,
queue_size=1)
def _twist_cmd_cb(self, msg):
"""
Callback registered to receive cmd_vel messages
Averages the receipt times of cmd_vel messages for use in velocity processing
Stores velocity for later processing
:param msg: the cmd_vel message
:return: None
"""
self._cmd_vel_state.add_delta(rospy.Time.now())
self._cmd_vel_state.velocity.set(v=msg.linear.x, w=msg.angular.z)
def _speedin_cb(self, msg):
"""
Callback registered to receive HALSpeedIn messages
Receives and stores unicycle speed
:param msg: the HALSpeedIn message (see HALSpeedIn.msg)
:return: None
"""
self._odometry.set_velocity(msg.linear, msg.angular)
def _positionin_cb(self, msg):
"""
Callback registered to receive HALPositionIn messages
Receives and stores x/y position
:param msg: the HALPositionIn message (see HALPositionIn.msg)
:return: None
"""
self._odometry.set_position(msg.x, msg.y)
def _headingin_cb(self, msg):
"""
:param msg:
:return:
"""
self._odometry.set_orientation(yaw=msg.heading)
def _eulerin_cb(self, msg):
"""
Callback registered to receive HALEulerIn messages
Receives and stores roll, pitch and yaw
:param msg: the HALEulerIn message
:return: None
"""
#self._odometry.set_orientation(msg.roll, msg.pitch, msg.yaw)
def _publish_odom(self):
"""
Publishes odometry on the odom topic
:return: None
"""
if self._odometry.is_ready():
self._odom_pub.publish(self._odometry.x, self._odometry.y,
self._odometry.velocity.linear,
self._odometry.velocity.angular,
self._odometry.heading)
def _velocity_has_changed(self):
return (not isclose(self._vel_state.target_velocity.linear,
self._vel_state.last_velocity.linear)
or not isclose(self._vel_state.target_velocity.angular,
self._vel_state.last_velocity.angular))
def _safety_check(self):
"""
Perform safety/sanity checking on the command velocity before passing through the velocity pipeline
if the command velocity is active (we're getting velocity commands) and command velocity is recent
- constrain the commanded velocity to mechanical and user-defined limits
- ensure requested angular velocity can be achieved
- store in target velocity
if the command velocity is stale or not being received at all
- zero out target velocity
:return: None
"""
# TODO-Add a velocity check for 'too' large velocity change with resulting adjustment that is proportional
if self._cmd_vel_state.is_active():
if self._cmd_vel_state.is_recent(num_msgs=5, max_time=2.0):
v_initial = self._cmd_vel_state.velocity.linear
w_initial = self._cmd_vel_state.velocity.angular
# Ensure linear and angular velocity are within mecahnical and user-defined ranges
v = constrain(v_initial, self._linear_max.backward,
self._linear_max.forward)
w = constrain(w_initial, self._angular_max.cw,
self._angular_max.ccw)
# Ensure the specified angular velocity can be achieved.
#
# Even though velocity is constrained within defined limits, it is necessary to check that the resulting
# velocities are achievable. For example if both wheels are moving at the maximum linear velocity and
# an angular velocity is specified, it is necessary to reduce the linear velocity in order to achieve
# the angular velocity.
# Note: Will adjust linear velocity if necessary
v, w = ensure_w(v, w, self._track_width, self._wheel_radius,
self._angular_max.wheel)
rospy.loginfo(
"SafetyCheck: initial {:.3f} {:.3f}, constrained {:.3f} {:.3f}"
.format(v_initial, w_initial, v, w))
self._vel_state.target_velocity.set(v, w)
else:
rospy.logwarn(
"No message received for {} msgs or {} secs".format(
5, 1.0))
self._vel_state.zero_target()
else:
rospy.logwarn("No command velocity active")
self._vel_state.zero_target()
def _limit_accel(self):
lv_initial = self._vel_state.last_velocity
tv_initial = self._vel_state.target_velocity
# Calculate the desired and max velocity increments for linear and angular velocities
v_inc, max_v_inc = calc_vel_inc(self._vel_state.target_velocity.linear,
self._vel_state.last_velocity.linear,
self._linear_max.accel,
self._linear_max.decel,
self._loop_period)
w_inc, max_w_inc = calc_vel_inc(
self._vel_state.target_velocity.angular,
self._vel_state.last_velocity.angular, self._angular_max.accel,
self._angular_max.decel, self._loop_period)
# Create normalized vectors for desired (v_inv/w_inc) and maximum (max_v_inc/max_w_inc) velocity increments
Av, Aw = normalize_vector(v_inc, w_inc)
Bv, Bw = normalize_vector(max_v_inc, max_w_inc)
# Use the angle between the vectors to determine which increment will dominate
theta = math.atan2(Bw, Bv) - math.atan2(Aw, Av)
if theta < 0.0:
try:
max_v_inc = (max_w_inc * math.fabs(v_inc)) / math.fabs(w_inc)
except ZeroDivisionError:
pass
else:
try:
max_w_inc = (max_v_inc * math.fabs(w_inc)) / math.fabs(v_inc)
except ZeroDivisionError:
pass
# Calculate the velocity delta to be applied
v_delta = math.copysign(1.0, v_inc) * min(math.fabs(v_inc),
math.fabs(max_v_inc))
w_delta = math.copysign(1.0, w_inc) * min(math.fabs(w_inc),
math.fabs(max_w_inc))
# Apply the velocity delta to the last velocity
self._vel_state.target_velocity.linear = self._vel_state.last_velocity.linear + v_delta
self._vel_state.target_velocity.angular = self._vel_state.last_velocity.angular + w_delta
rospy.logdebug("LimitAccel - initial {} {}, resulting {} {}".format(
lv_initial, tv_initial, self._vel_state.last_velocity,
self._vel_state.target_velocity))
def _track_velocity(self):
"""
Track linear and angular velocity using PID controller
Note: Only track when command velocity is active; otherwise, zero out published velocity
:return:
"""
rospy.logdebug("TV Entry - O: {}, T: {}, L: {}".format(
self._odometry.velocity, self._vel_state.target_velocity,
self._vel_state.last_velocity))
delta_v = self._lin_pid.update(self._odometry.velocity.linear)
delta_w = self._ang_pid.update(self._odometry.velocity.angular)
self._lin_pid.print(rospy.logdebug)
self._ang_pid.print(rospy.logdebug)
rospy.logdebug("TV - dv {:02.3f}, dw {:02.3f}".format(
delta_v, delta_w))
self._vel_state.target_velocity.linear += delta_v
self._vel_state.target_velocity.angular += delta_w
rospy.logdebug("TV Exit - O: {}, T: {}, L: {}".format(
self._odometry.velocity, self._vel_state.target_velocity,
self._vel_state.last_velocity))
def _process_velocity(self):
rospy.logdebug("PV Entry - T: {}, L: {}".format(
self._vel_state.target_velocity, self._vel_state.last_velocity))
self._safety_check()
if self._velocity_has_changed():
rospy.logdebug("Velocity Changed")
self._limit_accel()
else:
rospy.logdebug("Velocity Unchanged")
self._vel_state.target_velocity.linear = self._vel_state.last_velocity.linear
self._vel_state.target_velocity.angular = self._vel_state.last_velocity.angular
#self._lin_pid.set_target(self._vel_state.target_velocity.linear)
#self._ang_pid.set_target(self._vel_state.target_velocity.angular)
rospy.logdebug("PV Exit - T: {}, L: {}".format(
self._vel_state.target_velocity, self._vel_state.last_velocity))
def _update_speed(self):
linear, angular = 0.0, 0.0
if self._vel_state.target_velocity.linear != 0.0 or self._vel_state.target_velocity.angular != 0.0:
linear, angular = self._vel_state.target_velocity.linear, self._vel_state.target_velocity.angular
self._speedout_pub.publish(SpeedData(linear=linear, angular=angular))
def start(self):
rospy.loginfo("DriveNode starting ...")
self._speedout_pub.publish(SpeedData(linear=0.0, angular=0.0))
rospy.loginfo("DriveNode started")
def loop(self):
rospy.loginfo("DriveNode loop starting ...")
while not rospy.is_shutdown():
# Smooth velocity
self._process_velocity()
# Update Speed
self._update_speed()
# Track velocity
#self._track_velocity()
self._vel_state.last_velocity.linear = self._vel_state.target_velocity.linear
self._vel_state.last_velocity.angular = self._vel_state.target_velocity.angular
# Publish odometry
self._publish_odom()
rospy.loginfo(
"Linear(T/O): {:6.3f}/{:6.3f}, Angular(T/O): {:6.3f}/{:6.3f}".
format(self._vel_state.target_velocity.linear,
self._odometry.velocity.linear,
self._vel_state.target_velocity.angular,
self._odometry.velocity.angular))
self._loop_rate.sleep()
self.shutdown()
rospy.loginfo("DriveNode loop exiting")
def shutdown(self):
rospy.loginfo("DriveNode shutdown")
class DriveNode(BaseNode):
"""
Implements the Arlobot drive node
"""
def __init__(self, name, debug):
super(DriveNode, self).__init__(name=name, debug=debug)
self.rate = rospy.get_param('loop rate', 20.0)
self.timeout = rospy.get_param('timeout', 3.0)
self.cmd_v = 0
self.curr_v = 0
self.meas_v = 0
self.cmd_w = 0
self.curr_w = 0
self.meas_w = 0
self.meas_x = 0
self.meas_y = 0
self.meas_th = 0
self.now = rospy.Time.now()
self.last_cmd = rospy.Time.now()
self.period = 1.0 / self.rate
self._timer = rospy.Rate(self.rate)
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._odom_pub = OdometryPublisher()
self._speedout_pub = HALSpeedOutPublisher()
# Create list of broadcasters and add odometry
self._broadcasts = [
lambda: self._odom_pub.
publish(self.now, self.meas_x, self.meas_y, self.meas_v, self.
meas_w, self.meas_th), lambda: self._speedout_pub.publish(
SpeedData(linear=self.curr_v, angular=self.curr_w))
]
def _cmd_vel_callback(self, msg):
self.last_cmd = rospy.Time.now()
self.cmd_v = msg.linear.x
self.cmd_w = msg.angular.z
def _speedin_cb(self, msg):
self.meas_v = msg.linear
self.meas_w = msg.angular
def _positionin_cb(self, msg):
self.meas_x = msg.x
self.meas_y = msg.y
def _headingin_cb(self, msg):
self.meas_th = msg.heading
def _process(self):
self.curr_v = self.cmd_v
self.curr_w = self.cmd_w
# Safety check if communication is lost
if self.now > (self.last_cmd + rospy.Duration(self.timeout)):
self._stop()
def _stop(self):
self.curr_v = 0
self.curr_w = 0
def _broadcast(self):
for b in self._broadcasts:
b()
def start(self):
self._stop()
def loop(self):
while not rospy.is_shutdown():
self.now = rospy.Time.now()
self._process()
self._broadcast()
self._timer.sleep()
def shutdown(self):
self._stop()