def test_light_interface(light_name='head_green_light'):
"""Blinks a desired Light on then off."""
l = Lights()
rospy.loginfo("All available lights on this robot:\n{0}\n".format(
', '.join(l.list_all_lights())))
rospy.loginfo("Blinking Light: {0}".format(light_name))
initial_state = l.get_light_state(light_name)
on_off = lambda x: 'ON' if l.get_light_state(x) else 'OFF'
rospy.loginfo("Initial state: {0}".format(on_off(light_name)))
# invert light
state = not initial_state
l.set_light_state(light_name, state)
rospy.sleep(1)
rospy.loginfo("New state: {0}".format(on_off(light_name)))
# invert light
state = not state
l.set_light_state(light_name, state)
rospy.sleep(1)
rospy.loginfo("New state: {0}".format(on_off(light_name)))
# invert light
state = not state
l.set_light_state(light_name, state)
rospy.sleep(1)
rospy.loginfo("New state: {0}".format(on_off(light_name)))
# reset output
l.set_light_state(light_name, initial_state)
rospy.sleep(1)
rospy.loginfo("Final state: {0}".format(on_off(light_name)))
def run(self):
rate = rospy.Rate(100)
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0.5,
corner_distance=0.05)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
self.pose.header = Header(stamp=rospy.Time.now(), frame_id='base')
self.pose.pose.position.x = 0.0
self.pose.pose.position.y = -0.6
self.pose.pose.position.z = 0.5
self.pose.pose.orientation.x = 0.5
self.pose.pose.orientation.y = -0.5
self.pose.pose.orientation.z = 0.5
self.pose.pose.orientation.w = 0.5
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(self.pose, "right_hand", joint_angles)
self.waypoints.append(waypoint)
rospy.loginfo("Sending inital waypoint: %s",
self.waypoints[0].to_string())
traj.append_waypoint(self.waypoints[0].to_msg())
result = traj.send_trajectory()
if result is None:
rospy.logerr('Trajectory FAILED to send')
elif result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
traj.clear_waypoints()
l = Lights()
l_name = 'head_green_light'
initial_state = l.get_light_state(l_name)
for i in range(0, 2):
state = not initial_state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
state = not state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
l.set_light_state(l_name, True)
for i in range(0, 19):
self.gen_rand_waypoint()
sendCommand = True
while not rospy.is_shutdown():
traj.clear_waypoints()
for i in range(0, 19):
self.waypoints.pop(i)
self.gen_rand_waypoint()
for point in self.waypoints:
traj.append_waypoint(point.to_msg())
print(len(self.waypoints))
result = traj.send_trajectory(wait_for_result=True)
self.firstShutdown = True
def clean_shutdown():
if self.firstShutdown:
print("STOPPING TRAJECTORY")
traj.stop_trajectory()
traj.clear_waypoints()
l = Lights()
l.set_light_state('head_green_light', False)
self.firstShutdown = False
rospy.on_shutdown(clean_shutdown)
rate.sleep()
return