def __init__(self):
self.node_name = "visual_odometry_april_tags_node"
self.sub = rospy.Subscriber("~april_tags", AprilTags,
self.processAprilTags)
self.sub_wheels_cmd = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped,
self.processWheelsCmd)
#Init the file
self.rpkg = rospkg.RosPack()
self.filename = self.setupParameter(
"~filename",
self.rpkg.get_path('kinematics') +
'/include/kinematics/training_data.txt')
try:
os.remove(self.filename)
except OSError:
pass
# To store the wheels_cmd
self.wheels_cmd = WheelsCmdStamped()
# Initialize prev values
self.april_tags_prev = {}
self.timestamp_prev = rospy.Time()
self.wheels_cmd_prev = WheelsCmdStamped()
rospy.loginfo("[%s] has started", self.node_name)
def test_forward_kinematics_msg_throughput(self):
#Setup the publisher and subscribers
self.pub_wheels_cmd = rospy.Publisher("forward_kinematics_node/wheels_cmd", WheelsCmdStamped, queue_size=1, latch=True)
self.sub = rospy.Subscriber("forward_kinematics_node/velocity", Twist2DStamped, self.velocityCallback)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time()+10.0
while (self.sub.get_num_connections() < 1 or self.pub_wheels_cmd.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a wheels_cmd
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
# Wait for the message to be received
while not self.msg_received and not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Check the result
self.assertAlmostEqual(self.v, 2)
self.assertAlmostEqual(self.omega, 0)
def cbPose(self, lane_pose_msg):
self.lane_reading = lane_pose_msg
#rospy.loginfo(self.lane_reading.d)
self.phi = self.lane_reading.phi
self.d = self.lane_reading.d #3*(self.lane_reading.d -0.125)
self.list_d.append(self.d)
self.list_d.pop(0)
self.list_phi.append(self.phi)
self.list_phi.pop(0)
avg_d = sum(self.list_d) / float(len(self.list_d))
avg_phi = sum(self.list_phi) / float(len(self.list_phi))
rospy.loginfo("D: " + str(avg_d))
rospy.loginfo("Phi: " + str(avg_phi))
# Distance between wheels
b = 0.1
# Standard velocity
gain = 0.5
v0g = 0.2
v0 = 0.145 / 0.3 * v0g
gain_p = 2
#self.s = b/2/v0 * avg_d*gain - b*avg_phi*gain
self.s = b / 2 / v0 * self.d * gain - b * self.phi * gain_p
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = header
wcs.vel_left = v0g - self.s / 2
wcs.vel_right = v0g + self.s / 2
self.pub_wheels_cmd.publish(wcs)
def test_forward_kinematics_msg_throughput(self):
#Setup the publisher and subscribers
self.pub_wheels_cmd = rospy.Publisher(
"forward_kinematics_node/wheels_cmd",
WheelsCmdStamped,
queue_size=1,
latch=True)
self.sub = rospy.Subscriber("forward_kinematics_node/velocity",
Twist2DStamped, self.velocityCallback)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time() + 10.0
while (self.sub.get_num_connections() < 1 or self.pub_wheels_cmd.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a wheels_cmd
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
# Wait for the message to be received
while not self.msg_received and not rospy.is_shutdown(
) and not time.time() > timeout:
rospy.sleep(0.1)
# Check the result
self.assertAlmostEqual(self.v, 2)
self.assertAlmostEqual(self.omega, 0)
def stop(self):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = 0.0
msg.vel_right = 0.0
self.pub.publish(msg)
print("Stop")
def stop(self):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = 0.0
msg.vel_right = 0.0
self.wheelpub.publish(msg)
self.set_leds("stop")
def car_cmd_callback(self, msg_car_cmd):
# assuming same motor constants k for both motors
k_r = self.k
k_l = self.k
# adjusting k by gain and trim
k_r_inv = (self.gain + self.trim) / k_r
k_l_inv = (self.gain - self.trim) / k_l
omega_r = (msg_car_cmd.v + 0.5 * msg_car_cmd.omega * self.baseline) / self.radius
omega_l = (msg_car_cmd.v - 0.5 * msg_car_cmd.omega * self.baseline) / self.radius
# conversion from motor rotation rate to duty cycle
# u_r = (gain + trim) (v + 0.5 * omega * b) / (r * k_r)
u_r = omega_r * k_r_inv
# u_l = (gain - trim) (v - 0.5 * omega * b) / (r * k_l)
u_l = omega_l * k_l_inv
# limiting output to limit, which is 1.0 for the duckiebot
u_r_limited = max(min(u_r, self.limit), -self.limit)
u_l_limited = max(min(u_l, self.limit), -self.limit)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header.stamp = msg_car_cmd.header.stamp
msg_wheels_cmd.vel_right = u_r_limited
msg_wheels_cmd.vel_left = u_l_limited
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def publishStop(self):
# Pass wheel stopping cmd (vel(0,0))
stop_cmd = WheelsCmdStamped()
stop_cmd.vel_left = 0.0
stop_cmd.vel_right = 0.0
self.pub_wheels_cmd.publish(stop_cmd)
rospy.loginfo("Published stop_cmd")
def createWheelCmd(self, left, right):
wheels_cmd_msg = WheelsCmdStamped()
# spin right unless servoing or centered
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = left
wheels_cmd_msg.vel_right = right
return wheels_cmd_msg
def car_cmd_callback(self, msg_car_cmd):
# assuming same motor constants k for both motors
k_r = self.k
k_l = self.k
# adjusting k by gain and trim
k_r_inv = (self.gain + self.trim) / k_r
k_l_inv = (self.gain - self.trim) / k_l
omega_r = (msg_car_cmd.v +
0.5 * msg_car_cmd.omega * self.baseline) / self.radius
omega_l = (msg_car_cmd.v -
0.5 * msg_car_cmd.omega * self.baseline) / self.radius
# conversion from motor rotation rate to duty cycle
# u_r = (gain + trim) (v + 0.5 * omega * b) / (r * k_r)
u_r = omega_r * k_r_inv
# u_l = (gain - trim) (v - 0.5 * omega * b) / (r * k_l)
u_l = omega_l * k_l_inv
# limiting output to limit, which is 1.0 for the duckiebot
u_r_limited = max(min(u_r, self.limit), -self.limit)
u_l_limited = max(min(u_l, self.limit), -self.limit)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header.stamp = msg_car_cmd.header.stamp
msg_wheels_cmd.vel_right = u_r_limited
msg_wheels_cmd.vel_left = u_l_limited
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def run(self):
# publish message every 1/x second
car_cmd_msg = WheelsCmdStamped()
tnew = time.time()
rdm.seed()
#parameter to change amplitude of disturbance
gain = 15.0
while not rospy.is_shutdown():
randomrate = rdm.random()
randomrate = randomrate*4+0.5
rate = rospy.Rate(randomrate)
omegarand = gain*(rdm.random()-0.5)
car_cmd_msg.header.stamp = rospy.get_rostime()
car_cmd_msg.vel_left = self.vref + self.L * omegarand
car_cmd_msg.vel_right = self.vref - self.L * omegarand
self.pub_wheels_cmd.publish(car_cmd_msg)
msg1 = omegarand
rospy.loginfo("omegarand: %s" % msg1)
rate.sleep()
def sub_redline_cb(self, msg):
if msg.data == True:
drive_cmd = WheelsCmdStamped()
drive_cmd.vel_left = 0.0
drive_cmd.vel_right = 0.0
self.stop = True
self.pub_drive_cmd_.publish(drive_cmd)
def custom_shutdown(self):
stop_msg = WheelsCmdStamped()
stop_msg.header.stamp = rospy.get_rostime()
stop_msg.vel_left = 0.0
stop_msg.vel_right = 0.0
self.pub_wheels_cmd.publish(stop_msg)
rospy.sleep(0.5)
rospy.loginfo("Shutdown complete oder?")
def forward(self, velocity, time):
# print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = velocity
self.wheelpub.publish(msg)
rospy.sleep(time)
def rotate(self, time):
# print "Rotate"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = self.speed
msg.vel_right = -self.speed
self.wheelpub.publish(msg)
rospy.sleep(time)
def cbTimer(self, event):
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = header
wcs.vel_left = 0.2 + 0.15 * math.cos(time.time() * 2 * math.pi / 2)
wcs.vel_right = 0.2 + 0.05 * math.sin(time.time() * 2 * math.pi / 2)
self.pub_wheels_cmd.publish(wcs)
def cbWheelsCmd(self, msg):
trimmed_cmd = WheelsCmdStamped()
trimmed_cmd.vel_left = np.clip(msg.vel_left * (1.0 - self.trim), -1.0,
1.0)
trimmed_cmd.vel_right = np.clip(msg.vel_right * (1.0 + self.trim),
-1.0, 1.0)
trimmed_cmd.header.stamp = msg.header.stamp
self.pub_topic.publish(trimmed_cmd)
def pubWheelsCmd(self):
cmd_msg = self.cmd_dict[self.mode_msg.state]
if cmd_msg is None:
cmd_msg = WheelsCmdStamped()
cmd_msg.vel_left = 0.0
cmd_msg.vel_right = 0.0
cmd_msg.header.stamp = rospy.Time.now()
self.pub_wheels_cmd.publish(cmd_msg)
def right_turn(self, time):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = self.speed + 0.3
msg.vel_right = self.speed
self.wheelpub.publish(msg)
self.set_leds("right_turn")
rospy.sleep(time)
def forward(self, velocity, time):
print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = velocity
self.pub.publish(msg)
sleep(time)
print "distance:'%s'" % (velocity * time)
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." % (self.node_name))
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = h
wcs.vel_left = 0
wcs.vel_right = 0
self.pub_wheels_cmd.publish(wcs)
def stop(self):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = 0.0
msg.vel_right = 0.0
self.wheelpub.publish(msg)
message = "Stop"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def rotate(self, velocity, time):
print "Rotate"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = -velocity
self.pub.publish(msg)
sleep(time)
print "Rotate:done"
def execute(self, inputs, outputs, gvm):
drive_cmd = WheelsCmdStamped()
node = gvm.get_variable("Node", True)
wheels_cmd_pub = node.create_publisher(WheelsCmdStamped, '/None/wheels_driver_node/wheels_cmd', 10)
drive_cmd.vel_right = 0.8
drive_cmd.vel_left = 0.8
wheels_cmd_pub.publish(drive_cmd)
return "success"
def carCmdCallback(self, msg_car_cmd):
[d_L, d_R] = self.ik.evaluate(msg_car_cmd.omega, msg_car_cmd.v)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header = msg_car_cmd.header
msg_wheels_cmd.vel_left = d_L
msg_wheels_cmd.vel_right = d_R
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def forward(self, time):
# print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = self.speed
msg.vel_right = self.speed
self.wheelpub.publish(msg)
self.set_leds("forward")
rospy.sleep(time)
def carCmdCallback(self, msg_car_cmd):
[d_L, d_R] = self.ik.evaluate(msg_car_cmd.omega, msg_car_cmd.v)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header = msg_car_cmd.header
msg_wheels_cmd.vel_left = d_L
msg_wheels_cmd.vel_right = d_R
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def rotate(self, velocity, time):
# print "Rotate"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = -velocity
self.wheelpub.publish(msg)
rospy.sleep(time)
message = "Rotate:done"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def forward(self, velocity, time):
# print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = velocity
self.wheelpub.publish(msg)
rospy.sleep(time)
message = "distance: forward"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def threadSetSpeed(self):
#rospy.loginfo("[%s] Set speed " %(self.node_name))
left = int(-(self.v + self.omega) * self.kMaxPPMS)
right = int((self.v - self.omega) * self.kMaxPPMS)
self.ps.set_speed([left, right])
wheelcmd = WheelsCmdStamped()
wheelcmd.header.stamp = rospy.Time.now()
wheelcmd.vel_left = -left
wheelcmd.vel_right = right
self.pub_wheelcmd.publish(wheelcmd)
def continuous_publisher():
vel_pub = rospy.Publisher('/random/topic/', WheelsCmdStamped, queue_size=1)
rospy.init_node('continuous_test_publisher', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = WheelsCmdStamped()
msg.vel_left = np.random.random()
msg.vel_right = np.random.random()
vel_pub.publish(msg)
rate.sleep()
def test_v_dot_simple(self):
# publish wheel_cmd_executed
wheels_cmd = WheelsCmdStamped()
rate = rospy.Rate(10)
for i in range(10):
wheels_cmd.header.stamp = rospy.Time.now()
wheels_cmd.vel_left = 0.5
wheels_cmd.vel_right = 0.5
self.pub_wheels_cmd_executed.publish(wheels_cmd)
rate.sleep()
# publish LanePose
lane_pose = LanePose()
lane_pose.d = 0
lane_pose.sigma_d = 0
lane_pose.phi = 0
lane_pose.sigma_phi = 0
lane_pose.status = 0
lane_pose.in_lane = True
for i in [1, 2]:
lane_pose.header.stamp = rospy.Time.now()
self.pub_lane_pose.publish(lane_pose)
rate.sleep()
# publish StopLineReading
stop_line_reading = StopLineReading()
stop_line_reading.stop_line_detected = True
stop_line_reading.at_stop_line = False
stop_line_reading.stop_line_point = Point()
for x in [0.51, 0.5]:
stop_line_reading.header.stamp = rospy.Time.now()
stop_line_reading.stop_line_point.x = x
self.pub_stop_line_reading.publish(stop_line_reading)
rate.sleep()
# Wait for the odometry_training_pairs_node to finish starting up
timeout = time.time() + 2.0
while not self.sub_v_sample.get_num_connections() and \
not rospy.is_shutdown() and not time.time() > timeout:
rospy.sleep(0.1)
msg = self.v_received
# self.assertEqual(hasattr(msg,'d_L'),True)
self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
self.assertAlmostEqual(msg.dt, 0.1, 2, 'dt = %s' % msg.dt)
self.assertEqual(
msg.theta_angle_pose_delta, 0,
'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5,
'x = %s' % msg.x_axis_pose_delta)
self.assertEqual(msg.y_axis_pose_delta, 0,
'y = %s' % msg.y_axis_pose_delta)
def right_turn(self, velocity, time):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity + 0.3
msg.vel_right = velocity
self.wheelpub.publish(msg)
self.set_leds("right_turn")
rospy.sleep(time)
message = "right_turn"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def publishControl(self):
speed = self.joy.axes[1] * self.speed_gain #Left stick V-axis. Up is positive
steering = self.joy.axes[3] * self.steer_gain
wheels_cmd_msg = WheelsCmdStamped()
# Car Steering Mode
vel_left = (speed - steering)
vel_right = (speed + steering)
wheels_cmd_msg.header.stamp = self.joy.header.stamp
wheels_cmd_msg.vel_left = np.clip(vel_left,-1.0,1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right,-1.0,1.0)
rospy.loginfo("[%s] left %f, right %f" % (self.node_name,wheels_cmd_msg.vel_left,wheels_cmd_msg.vel_right))
self.pub_wheels.publish(wheels_cmd_msg)
def test_v_dot_simple(self):
# publish wheel_cmd_executed
wheels_cmd = WheelsCmdStamped()
rate = rospy.Rate(10)
for i in range(10):
wheels_cmd.header.stamp = rospy.Time.now()
wheels_cmd.vel_left = 0.5
wheels_cmd.vel_right = 0.5
self.pub_wheels_cmd_executed.publish(wheels_cmd)
rate.sleep()
# publish LanePose
lane_pose = LanePose()
lane_pose.d = 0
lane_pose.sigma_d = 0
lane_pose.phi = 0
lane_pose.sigma_phi = 0
lane_pose.status = 0
lane_pose.in_lane = True
for i in [1, 2]:
lane_pose.header.stamp = rospy.Time.now()
self.pub_lane_pose.publish(lane_pose)
rate.sleep()
# publish StopLineReading
stop_line_reading = StopLineReading()
stop_line_reading.stop_line_detected = True
stop_line_reading.at_stop_line = False
stop_line_reading.stop_line_point = Point()
for x in [0.51, 0.5]:
stop_line_reading.header.stamp = rospy.Time.now()
stop_line_reading.stop_line_point.x = x
self.pub_stop_line_reading.publish(stop_line_reading)
rate.sleep()
# Wait for the odometry_training_pairs_node to finish starting up
timeout = time.time()+2.0
while not self.sub_v_sample.get_num_connections() and \
not rospy.is_shutdown() and not time.time() > timeout:
rospy.sleep(0.1)
msg = self.v_received
# self.assertEqual(hasattr(msg,'d_L'),True)
self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
self.assertAlmostEqual(msg.dt, 0.1, 2,'dt = %s' % msg.dt)
self.assertEqual(msg.theta_angle_pose_delta, 0, 'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5, 'x = %s' % msg.x_axis_pose_delta)
self.assertEqual(msg.y_axis_pose_delta, 0, 'y = %s' % msg.y_axis_pose_delta)
def processBag(self):
count = 0
with rosbag.Bag(self.bagout, 'w') as outbag:
for topic, msg, t in rosbag.Bag(self.bagin).read_messages():
if rospy.is_shutdown():
break
if topic == self.topicin:
msgout = WheelsCmdStamped()
msgout.vel_left = msg.vel_left
msgout.vel_right = msg.vel_right
msgout.header.stamp = t
outbag.write(self.topicout, msgout, t)
count += 1
else:
outbag.write(topic, msg, t)
return count
def test_april_tags_single_interval(self):
#Setup the publisher and subscribers
self.pub_april_tags = rospy.Publisher("visual_odometry_april_tags_node/april_tags", AprilTags, queue_size=1, latch=True)
self.pub_wheels_cmd = rospy.Publisher("visual_odometry_april_tags_node/wheels_cmd", WheelsCmdStamped, queue_size=1, latch=True)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time()+5.0
while (self.pub_wheels_cmd.get_num_connections() < 1 or self.pub_april_tags.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a single wheels cmd, and two simple april tag messages
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(0.2) #Wait so the tags come in the right order
T1 = Transform()
T2 = Transform()
T1.translation.y = 2
T2.translation.y = 2
T2.rotation.x, T2.rotation.y, T2.rotation.z, T2.rotation.w = tr.quaternion_about_axis(-np.pi/2, (0,0,1))
msg_tag1 = AprilTags()
tag = TagDetection()
tag.transform = T1
msg_tag1.detections.append(tag)
msg_tag1.header.stamp = rospy.Duration(0)
self.pub_april_tags.publish(msg_tag1)
rospy.sleep(0.2) #Wait so the tags come in the right order
msg_tag2 = AprilTags()
msg_tag1.header.stamp = rospy.Duration(1)
tag.transform = T2
msg_tag1.detections.append(tag)
self.pub_april_tags.publish(msg_tag1)
# Wait 1 second for the file to be output
rospy.sleep(3)
res = np.genfromtxt(rospy.get_param("visual_odometry_april_tags_node/filename", ''))
assert_almost_equal(res, np.array([1,1,1,np.pi/2, 2, 2]))
def donot_test_vicon_learning_sample_calculation(self):
self.setup_publisher_and_subscriber()
# publish the first wheels_cmd
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
# publish a vicon pose
msg_pose = PoseStamped()
msg_pose.pose.position.x = 1
msg_pose.pose.position.y = 1
q = tr.quaternion_about_axis(np.pi/2,(0,0,1))
for i, attr in enumerate(['x', 'y', 'z', 'w']):
msg_pose.pose.orientation.__setattr__(attr, q[i])
self.pub_pose.publish(msg_pose)
# publish second wheels_cmd pose
msg_wheels_cmd.header.stamp = rospy.Time(1)
self.pub_wheels_cmd.publish(msg_wheels_cmd)
# Wait for the samples to come back
timeout = time.time()+10.0
while not (self.msg_v_received and self.msg_theta_dot_received) and not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Notify if the timeout struck
self.assertLess(time.time(),timeout,msg="WARNING: Timeout reached, no samples received from vicon_learning_node.")
self.assertAlmostEqual(self.msg_v_sample.d_R, 1)
self.assertAlmostEqual(self.msg_v_sample.d_L, 1)
self.assertAlmostEqual(self.msg_v_sample.dt, 1)
self.assertAlmostEqual(self.msg_v_sample.x_axis_pose_delta, -1)
self.assertAlmostEqual(self.msg_v_sample.y_axis_pose_delta, 1)
self.assertAlmostEqual(self.msg_v_sample.theta_angle_pose_delta, np.pi/2)
self.assertAlmostEqual(self.msg_theta_dot_sample.d_R, 1)
self.assertAlmostEqual(self.msg_theta_dot_sample.d_L, 1)
self.assertAlmostEqual(self.msg_theta_dot_sample.dt, 1)
self.assertAlmostEqual(self.msg_theta_dot_sample.theta_angle_pose_delta, np.pi/2)
def turnRight(self):
#move forward
forward_for_time_leave = 2.0
turn_for_time = 0.7
forward_for_time_enter = 2.0
starting_time = rospy.Time.now()
while((rospy.Time.now() - starting_time) < rospy.Duration(forward_for_time_leave)):
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = 0.4
wheels_cmd_msg.vel_right = 0.4
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
self.rate.sleep()
#turn right
starting_time = rospy.Time.now()
while((rospy.Time.now() - starting_time) < rospy.Duration(turn_for_time)):
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = 0.25
wheels_cmd_msg.vel_right = -0.25
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
self.rate.sleep()
#coordination with lane controller means part way through announce finished turn
self.pub_wheels_done.publish(True)
#move forward
starting_time = rospy.Time.now()
while((rospy.Time.now() - starting_time) < rospy.Duration(forward_for_time_enter)):
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = 0.4
wheels_cmd_msg.vel_right = 0.4
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
self.rate.sleep()
def cbWheelsCmd(self,msg):
trimmed_cmd = WheelsCmdStamped()
trimmed_cmd.vel_left = np.clip(msg.vel_left*(1.0-self.trim),-1.0,1.0)
trimmed_cmd.vel_right = np.clip(msg.vel_right*(1.0+self.trim),-1.0,1.0)
trimmed_cmd.header.stamp = msg.header.stamp
self.pub_topic.publish(trimmed_cmd)
def servo(self):
#move forward
#end = rospy.Time.now() + rospy.Duration(.5)
# continuous spin until servo line detected
centered = True
while not rospy.is_shutdown():
wheels_cmd_msg = WheelsCmdStamped()
# spin right unless servoing or centered
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = .2
wheels_cmd_msg.vel_right = -.2
angle_direction = (self.ibvs_data - 0.5)
gain = .1
if self.ibvs_data == -1:
rospy.loginfo("No Line Detected! continuing turn")
self.pub_servo_status.publish(String(data="None"))
elif abs(angle_direction) < 0.05:
rospy.loginfo("Centered!")
wheels_cmd_msg.vel_left =0
wheels_cmd_msg.vel_right =0
centered = True
self.pub_servo_status.publish(String(data="center"))
elif angle_direction > 0:
wheels_cmd_msg.vel_left = gain#*abs(angle_direction)
wheels_cmd_msg.vel_right = -gain# *abs(angle_direction)
rospy.loginfo("Servo right %f " % angle_direction)
self.pub_servo_status.publish(String(data="moving"))
else:
wheels_cmd_msg.vel_right = gain# *abs(angle_direction)
wheels_cmd_msg.vel_left = -gain# *abs(angle_direction)
rospy.loginfo("Servo left %f " % angle_direction)
self.pub_servo_status.publish(String(data="moving"))
self.pub_wheels_cmd.publish(wheels_cmd_msg)
#self.rate.sleep()
if centered:
rospy.loginfo("centered. Waiting for while")
rospy.sleep(rospy.Duration(2.5))
# while not rospy.is_shutdown():
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = .3
wheels_cmd_msg.vel_right = -.3
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.sleep(rospy.Duration(0.5))
centered = False
def servo(self, turn_type):
#move forward
wheels_cmd_msg = WheelsCmdStamped()
end_time = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now() < end_time:
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = .4 # go straight
wheels_cmd_msg.vel_right = .4
self.pub_cmd.publish(wheels_cmd_msg)
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
while not rospy.is_shutdown():
#self.mode == "INTERSECTION_CONTROL": # If not in the mode anymore, return
angle_direction = (0.5 - self.ibvs_data)
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = 0
wheels_cmd_msg.vel_right = 0
gain = 0.5
done = False
if abs(angle_direction) < 0.1 or self.ibvs_data == -1:
if self.ibvs_data == -1:
rospy.loginfo("nothing detected!")
wheels_cmd_msg.vel_left = .2 # go straight
wheels_cmd_msg.vel_right = -.2
else:
rospy.loginfo("already centered!")
done= True
wheels_cmd_msg.vel_left = .4 # go straight
wheels_cmd_msg.vel_right = .4
else:
if angle_direction > 0:
wheels_cmd_msg.vel_left = gain*abs(angle_direction)
rospy.loginfo("turning left %f " % angle_direction)
else:
wheels_cmd_msg.vel_right = gain*abs(angle_direction)
rospy.loginfo("turning right %f " % angle_direction)
self.pub_cmd.publish(wheels_cmd_msg)
if self.in_lane:# and done==True:
self.pub_done.publish(msg)
self.rate.sleep()
