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 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 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 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()
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 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 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 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 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 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 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 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 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 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")
message = "stop"
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 listener(msg):
global should_take_photo
vel_msg = WheelsCmdStamped()
if any(info.traffic_sign_type==5 for info in msg.infos):
os.system("~/killThing.sh")
time.sleep(3.63)
vel_msg.vel_right = 0.0
vel_msg.vel_left = 0.0
time.sleep(0.1)
vel.publish(vel_msg)
the_thing('JOYSTICK_CONTROL')
vel.publish(vel_msg)
should_take_photo=True
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 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 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 continuous_publisher():
vehicle = os.getenv("VEHICLE_NAME")
topic = "/{}/wheels_driver_node/wheels_cmd".format(vehicle)
vel_pub = rospy.Publisher(topic, WheelsCmdStamped, queue_size=1)
rospy.init_node("random_action_node", 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 send_commands(self, wheel_cmds_dict):
vel_right_array = wheel_cmds_dict['right_wheel']
vel_left_array = wheel_cmds_dict['left_wheel']
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
for i in range(len(vel_right_array)):
msg_wheels_cmd.header.stamp = rospy.Time.now()
msg_wheels_cmd.vel_right = vel_right_array[i]
msg_wheels_cmd.vel_left = vel_left_array[i]
#rospy.loginfo("Left Wheel: {} \t Right Wheel: {}".format(vel_left_array[i], vel_right_array[i]))
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(1 / float(self.frequency))
def send_commands(self, cmds):
"""
Publishes the wheel commands to ROS
"""
time = rospy.get_rostime()
cmd_msg = WheelsCmdStamped()
cmd_msg.header.stamp.secs = time.secs
cmd_msg.header.stamp.nsecs = time.nsecs
cmd_msg.vel_right = cmds[u'motor_right']
cmd_msg.vel_left = cmds[u'motor_left']
if self.nsent_commands == 0:
msg = 'ROSClient publishing first commands.'
logger.info(msg)
self.cmd_pub.publish(cmd_msg)
self.nsent_commands += 1
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 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 run(self):
# publish message every 1/x second
rate = rospy.Rate(10)
car_cmd_msg = WheelsCmdStamped()
tnew = time.time()
stoptime = 25.0
t0 = time.time()
while not rospy.is_shutdown():
#stop programm once a certain time has passed (for experiments, not meant for normal usage)
if time.time() - t0 > stoptime:
rospy.logwarn("Time's up!!!")
rospy.signal_shutdown("Ende gut, alles gut")
self.custom_shutdown()
#computing dt for I-part of controller
told = tnew
tnew = time.time()
dt = tnew - told
#self.vdiff = self.getvdiff(self.dist,self.tist,dt)
self.omega = self.getomega(self.dist, self.tist, dt)
#def. motor commands that will be published
car_cmd_msg.header.stamp = rospy.get_rostime()
car_cmd_msg.vel_left = self.vref + self.L * self.omega
car_cmd_msg.vel_right = self.vref - self.L * self.omega
self.pub_wheels_cmd.publish(car_cmd_msg)
#printing messages to verify that program is working correctly
#i.ei if dist and tist are always zero, then there is probably no data from the lan_pose
message1 = self.dist
message2 = self.omega
message3 = self.tist
message4 = dt
message5 = time.time() - t0
#rospy.loginfo('d: %s' % message1)
#rospy.loginfo('phi: %s' % message3)
#rospy.loginfo('dt: %s' % message4)
#rospy.loginfo("time: %s" % message5)
rate.sleep()
def publishCmd(self, stamp, speed, steering):
# lane_cmd_msg = CarControl()
# lane_cmd_msg.header.stamp = stamp
# lane_cmd_msg.speed = speed
# lane_cmd_msg.steering = steering
# lane_cmd_msg.need_steering = True
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = stamp
speed_gain = 1.0
steer_gain = 0.5
vel_left = (speed_gain * speed - steer_gain * steering)
vel_right = (speed_gain * speed + steer_gain * steering)
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)
# self.pub_lane_cmd.publish(lane_cmd_msg)
self.pub_wheels_cmd.publish(wheels_cmd_msg)
def cbDetections(self, detections_msg):
if self.too_close:
minDist = 999999999999999999999.0
offset = 0.0
# + -> offset to left in lane
# - -> offset to right in lane
for projected in detections_msg.list:
# ~0.23 is the lane width
# goes through each object- duckie or cone
# add if statement here to handle duckie different than cone
if projected.type.type == ObstacleType.DUCKIE:
stop = WheelsCmdStamped()
stop.header = bool_msg.header
stop.vel_left = 0.0
stop.vel_right = 0.0
#stop while the object is still there, and move when the coast is clear
else:
if projected.distance < minDist and abs(
projected.location.y) < 0.15:
minDist = projected.distance
y = projected.location.y
# + y -> obstacle to the left, drive right (set offset negative)
# - y -> obstacle to the right, drive left (set offset positive)
if y > 0:
offset = (-0.15 - y) * 1.5
else:
offset = (0.15 - y) * 1.5
# add else here
#Hijack the param for seting offset of the lane
rospy.set_param("lane_controller_node/d_offset", offset)
else:
#Reset offset of lane to 0
rospy.set_param("lane_controller_node/d_offset", 0.0)
# stop = WheelsCmdStamped()
# stop.header = bool_msg.header
# stop.vel_left = 0.0
# stop.vel_right = 0.0
# Slow it down so it's easier to see what's going on for now
self.lane_control.vel_left = self.lane_control.vel_left
self.lane_control.vel_right = self.lane_control.vel_right
self.pub_wheels_cmd.publish(self.lane_control)
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 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 run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#rospy.loginfo("Hallo\n")
car_cmd_msg = WheelsCmdStamped()
car_cmd_msg.header.stamp = rospy.get_rostime()
car_cmd_msg.vel_left = self.vref - self.L * self.omega
car_cmd_msg.vel_right = self.vref + self.L * self.omega
'''
msg1 = self.omega
msg2 = self.vref
rospy.loginfo("omega: %s" % msg1)
rospy.loginfo("vref: %s" % msg2)
'''
# Send the command to the car
self.pub_wheels_cmd.publish(car_cmd_msg)
rate.sleep()
def run(self):
# publish message every 1/x second
rate = rospy.Rate(2)
car_cmd_msg = WheelsCmdStamped()
tnew = time.time()
while not rospy.is_shutdown():
#computing dt for I-part of controller
told = tnew
tnew = time.time()
dt = tnew-told
phiref, vref = self.getphiref(self.dist,self.phi)
err = phiref - self.phi
self.omega = self.getomega(err,dt)
#def. motor commands that will be published
car_cmd_msg.header.stamp = rospy.get_rostime()
car_cmd_msg.vel_left = vref - self.L * self.omega
car_cmd_msg.vel_right = vref + self.L * self.omega
self.pub_wheels_cmd.publish(car_cmd_msg)
#printing messages to verify that program is working correctly
#i.ei if dist and tist are always zero, then there is probably no data from the lan_pose
message1 = self.dist
message2 = self.omega
message3 = phiref
message4 = vref
rospy.loginfo('d: %s' % message1)
rospy.loginfo('phiref: %s' % message3)
rospy.loginfo('vref: %s' % message4)
rospy.loginfo('omega: %s' % message2)
rate.sleep()
def run(self):
# publish message every 1/x second
#for cascade choose rate > rate of camera (rate of camera 8-35 Hz)
rate = rospy.Rate(100)
car_cmd_msg = WheelsCmdStamped()
tnew = time.time()
while not rospy.is_shutdown():
#computing dt for I and D-part of controller
told = tnew
tnew = time.time()
dt = tnew - told
phiref = self.getphiref(self.dist)
self.omega = self.getomega(phiref, self.phiist, dt)
#def. motor commands that will be published
car_cmd_msg.header.stamp = rospy.get_rostime()
car_cmd_msg.vel_left = self.vref + self.L * self.omega
car_cmd_msg.vel_right = self.vref - self.L * self.omega
self.pub_wheels_cmd.publish(car_cmd_msg)
#printing messages to verify that program is working correctly
#i.ei if dist and tist are always zero, then there is probably no data from the lan_pose
message1 = self.dist
message2 = self.omega
message3 = self.phiist
message4 = phiref
message5 = dt
#rospy.loginfo('d: %s' % message1)
#rospy.loginfo('phi: %s' % message3)
#rospy.loginfo('phiref: %s' % message4)
#rospy.loginfo('omega: %s' % message2)
'''
self.time_arr.append(dt)
rospy.logwarn("dt: %s" % message5)
'''
rate.sleep()
def run(self):
# publish message every 1/x second
rate = rospy.Rate(10)
car_cmd_msg = WheelsCmdStamped()
tnew = time.time()
while not rospy.is_shutdown():
self.vdiff = self.getvdiff(self.dist)
#def. motor commands that will be published
car_cmd_msg.header.stamp = rospy.get_rostime()
car_cmd_msg.vel_left = self.vref + self.vdiff
car_cmd_msg.vel_right = self.vref - self.vdiff
self.pub_wheels_cmd.publish(car_cmd_msg)
#printing messages to verfy that program is working
message1 = self.dist
message2 = self.vdiff
rospy.loginfo('dist: %s' % message1)
rospy.loginfo('vdiff1: %s' % message2)
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 car_cmd_callback(self, msg_car_cmd):
"""
A callback that reposponds to received `car_cmd` messages by calculating the
corresponding wheel commands, taking into account the robot geometry, gain and trim
factors, and the set limits. These wheel commands are then published for the motors to use.
The resulting linear and angular velocities are also calculated and published.
Args:
msg_car_cmd (:obj:`Twist2DStamped`): desired car command
"""
# INVERSE KINEMATICS PART
# trim the desired commands such that they are within the limits:
msg_car_cmd.v = self.trim(
msg_car_cmd.v,
low=-self._v_max,
high=self._v_max
)
msg_car_cmd.omega = self.trim(
msg_car_cmd.omega,
low=-self._omega_max,
high=self._omega_max
)
# 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.value + self._trim.value) / k_r
k_l_inv = (self._gain.value - self._trim.value) / 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 = self.trim(u_r, -self._limit.value, self._limit.value)
u_l_limited = self.trim(u_l, -self._limit.value, self._limit.value)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header.stamp = msg_car_cmd.header.stamp
if u_r_limited > 0 and u_r_limited < 0.1:
u_r_limited = 0.1
if u_r_limited < 0 and u_r_limited > -0.1:
u_r_limited = -0.1
if u_l_limited > 0 and u_l_limited < 0.1:
u_l_limited = 0.1
if u_l_limited < 0 and u_l_limited > -0.1:
u_l_limited = -0.1
msg_wheels_cmd.vel_right = u_r_limited
msg_wheels_cmd.vel_left = u_l_limited
self.pub_wheels_cmd.publish(msg_wheels_cmd)
# FORWARD KINEMATICS PART
# Conversion from motor duty to motor rotation rate
omega_r = msg_wheels_cmd.vel_right / k_r_inv
omega_l = msg_wheels_cmd.vel_left / k_l_inv
# Compute linear and angular velocity of the platform
v = (self._radius * omega_r + self._radius * omega_l) / 2.0
omega = (self._radius * omega_r - self._radius * omega_l) / self._baseline
# Put the v and omega into a velocity message and publish
msg_velocity = Twist2DStamped()
msg_velocity.header = msg_wheels_cmd.header
msg_velocity.v = v
msg_velocity.omega = omega
self.pub_velocity.publish(msg_velocity)
