def _get_command(direction_to_speed):
""" Converts mappings such as { right: 50 } into appropriate OverrideRCIn command.
The input dictionary CAN have multiple key-value pairs. """
cmd = neutral_command()
display_string = ""
for direction, speed, in direction_to_speed.iteritems():
channel, modifier = CHANNEL_INFO[direction]
cmd.channels[channel] = RC_NEUTRAL + speed * modifier
display_string += direction + ": " + str(speed) + "\t"
print(display_string)
return cmd
def move(direction, duration_seconds = 1, speed=50):
""" Moves in a single direction, as specified by a string
(e.g., "forward", or "right"). """
movement = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
establish_QGC_control()
setMode("MANUAL")
armRobot()
cmd = _get_command({direction:speed})
start = rospy.get_time()
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown() and rospy.get_time() - start < duration_seconds:
movement.publish(cmd)
rate.sleep()
movement.publish(neutral_command())
def move(self, direction, duration_seconds=1, speed=50):
""" Moves in a single direction, as specified by a string
(e.g., "forward", or "right"). """
establish_QGC_control()
setMode("MANUAL")
armRobot()
cmd = self._get_command({direction: speed})
start = rospy.get_time()
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown(
) and rospy.get_time() - start < duration_seconds:
self.movement.publish(cmd)
rate.sleep()
self.movement.publish(neutral_command())
def move_to(self, pose, max_duration=10):
""" Here, pose is a tuple of numpy arrays: (position, orientation).
max_duration is in seconds. """
establish_QGC_control()
setMode("MANUAL")
armRobot()
self.goal_pose = pose
start = rospy.get_time()
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown(
) and rospy.get_time() - start < max_duration:
if self.direction_to_speed == None or len(
self.poses[-1]) != 3: # TODO clean this printing.
continue
display_string = ""
for direction, speed, in self.direction_to_speed.iteritems():
display_string += direction + ": {0:6.2f}".format(speed) + "\t"
print(display_string)
_, curr_xyz, curr_orient = self.poses[-1]
goal_xyz, goal_orient = self.goal_pose
delta_xyz = goal_xyz - curr_xyz
delta_orient = goal_orient - curr_orient
print("\t\t\t\t\t\t\t\t\t\t" + array_to_string(delta_xyz) +
array_to_string(delta_orient))
# TODO have we reached goal pose?
# TODO break loop
if self.move_cmd is not None:
self.movement.publish(self.move_cmd)
rate.sleep()
if rospy.get_time() - start >= max_duration:
print("Stopped because the maximum duration was exceeded; " +
"may NOT have reached the position. ")
self.goal_pose = None
self.movement.publish(neutral_command())