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 __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 __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, 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)