def _exec_action(action_client: SimpleActionClient, goal, stopping_event: EventBase = None):
"""Executes goal with specified action client and optional event that stops action execution when triggered.
This is a private method.
:param action_client: Action client to use.
:param goal: Action goal to execute.
:param stopping_event: Optional event that stops goal execution when triggered.
:return: None if stopping_event is None. True if goal execution is stopped because of triggered stopping_event,
False otherwise.
"""
if stopping_event is None:
action_client.send_goal(goal)
action_client.wait_for_result()
return
stopping_event.start_listening()
action_client.send_goal(goal,
active_cb=None,
feedback_cb=None,
done_cb=None)
check_rate = rospy.Rate(100)
while True:
goal_state = action_client.get_state()
if goal_state == SimpleGoalState.DONE:
stopping_event.stop_listening()
return False
if stopping_event.is_triggered():
action_client.cancel_goal()
return True
check_rate.sleep()
class ReadyArmActionServer:
def __init__(self):
self.move_arm_client = SimpleActionClient('/move_left_arm',
MoveArmAction)
self.ready_arm_server = SimpleActionServer(ACTION_NAME,
ReadyArmAction,
execute_cb=self.ready_arm,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for move_left_arm action server',
ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server',
ACTION_NAME)
self.ready_arm_server.start()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()):
move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
move_arm_client.wait_for_server()
rospy.loginfo('connected to move_left_arm action server')
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
goal.motion_plan_request.allowed_contacts = allowed_contacts
goal.motion_plan_request.link_padding = link_padding
goal.motion_plan_request.ordered_collision_operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.loginfo('timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('action finished: %s' % str(state))
return True
else:
rospy.loginfo('action failed: %s' % str(state))
return False
def head_action(self, x, y, z, wait=False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(
rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Head movement goal failed with error code: " +
str(self.goal_states[state]))
class ReadyArmActionServer:
def __init__(self):
self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
self.ready_arm_server = SimpleActionServer(ACTION_NAME,
ReadyArmAction,
execute_cb=self.ready_arm,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME)
self.ready_arm_server.start()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
class safe_land(object):
'''
Allows a human to teleoperate the robot through a joystick.
'''
def __init__(self, name):
self.robot_name = name
self.state = 'IDLE'
# Landing service
self._land_client = SimpleActionClient(
"safe_land_server",
ExecuteLandAction) # Create a client to the v_search server
self._land_client.wait_for_server() # Wait server to be ready
self._land_goal = ExecuteLandGoal() # Message to send the goal region
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def execute(self):
rospy.loginfo("Starting Safe Land!")
self.state = 'EXE'
# Start safe_land
self._land_client.send_goal(self._land_goal)
self._land_client.wait_for_result()
state = self._land_client.get_state() # Get the state of the action
print(state)
if state == GoalStatus.SUCCEEDED:
# safe_land successfully executed
self.state = 'IDLE' # Set IDLE state
self.msg.event = 'end_safe_land'
self.pub.publish(
self.msg
) # Send the message signaling that the safe_land is complete
else:
# The server aborted the motion
self.state = 'ERROR' # Set ERROR state
self.msg.event = 'safe_land_error'
self.pub.publish(self.msg) # Send message signaling the error
def reset(self):
rospy.loginfo("Reseting Safe Land!")
self.state = 'IDLE'
def erro(self):
rospy.loginfo("Safe Land Erro!")
self.state = 'ERROR' # Set ERROR state
self._land_client.cancel_goal() # Cancel the motion of the robot
def head_action(self, x, y, z, wait = False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
class Head(object):
def __init__(self):
self.head_client = SimpleActionClient(HEAD_CLIENT_TOPIC,
FollowJointTrajectoryAction)
self.head_state = rospy.Subscriber(HEAD_STATE_TOPIC,
JointTrajectoryControllerState,
self._update_state)
rospy.loginfo("HEAD CLIENT: Waiting for head action server...")
head_client_running = self.head_client.wait_for_server(
rospy.Duration(2))
if head_client_running:
rospy.loginfo("HEAD CLIENT: Head controller initialized.")
else:
rospy.loginfo("HEAD CLIENT: Head controller is NOT initialized!")
# Setup
self.head_goal = FollowJointTrajectoryGoal()
self.head_goal.trajectory.joint_names = HEAD_JOINTS
self.p = JointTrajectoryPoint()
self.current_tilt = 0.0
self.current_pan = 0.0
self.timeout = rospy.Duration(30)
# Pre-def. positions
r = RosPack()
self.yaml_filename = r.get_path(
'frasier_utilities') + '/config/head_configs.yaml'
# Read
with open(self.yaml_filename, 'r') as outfile:
try:
self.locations = yaml.load(outfile)
except yaml.YAMLError as exc:
rospy.logwarn(
"HEAD CLIENT: Yaml Exception Caught: {}".format(exc))
rospy.logdebug("HEAD CLIENT: Config: {}".format(self.locations))
def _save_yaml_file(self):
with open(self.yaml_filename, 'w') as outfile:
yaml.dump(self.locations, outfile, default_flow_style=False)
def _read_yaml_file(self):
with open(self.yaml_filename, 'r') as outfile:
try:
self.locations = yaml.load(outfile)
except yaml.YAMLError as e:
print e
def _update_state(self, msg):
self.current_tilt, self.current_pan = msg.actual.positions
def save_position(self, name=None, pan=None, tilt=None):
# Handle all possible scenarios
if name is None:
if pan is None or tilt is None:
self.locations['default'] = [
self.current_pan, self.current_tilt
]
else:
self.locations['default'] = [pan, tilt]
else:
if pan is None or tilt is None:
self.locations[name] = [self.current_pan, self.current_tilt]
else:
self.locations[name] = [pan, tilt]
self._save_yaml_file()
return True
def send_goal(self, head_goal=None, blocking=True):
if head_goal is None:
self.head_goal.trajectory.header.stamp = rospy.Time.now()
self.head_client.send_goal(self.head_goal)
else:
self.head_client.send_goal(head_goal)
if blocking:
rospy.loginfo("HEAD CLIENT: Waiting for goal to complete...")
result = self.head_client.wait_for_result(self.timeout)
if not result:
rospy.logwarn("HEAD CLIENT: Goal timed out, canceled!")
self.head_client.cancel_goal()
return False
else:
state = self.head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("HEAD CLIENT: Goal completed.")
return True
else:
rospy.logwarn("HEAD CLIENT: Goal failed!")
return False
# Assume it succeeded
return True
def set_point(self, point=None):
if point is None:
self.head_goal.trajectory.points = [self.p]
else:
self.head_goal.trajectory.points = [point]
def move_to_location(self, location, time=3, blocking=True):
if location in self.locations:
self.p.positions = self.locations[location]
self.p.time_from_start = rospy.Time(time)
self.set_point()
return self.send_goal(blocking=blocking)
else:
rospy.logwarn(
"HEAD CLIENT: {} does not exist in the config file!".format(
location))
return False
# Pan: [-3.84,1.75] [right,left]
# Tilt: [-1.57,0.52] [down,up]
def move_to_position(self,
pan=None,
tilt=None,
velocities=None,
move_time=1,
blocking=True):
if pan is None:
pan = self.current_pan
if tilt is None:
tilt = self.current_tilt
if velocities is None:
velocities = [0, 0]
rospy.loginfo("HEAD CLIENT: Moving to {}, {} (Pan, Tilt)".format(
pan, tilt))
self.p.positions = [pan, tilt]
self.p.velocities = velocities
self.p.time_from_start = rospy.Time(move_time)
self.set_point()
return self.send_goal(blocking=blocking)
def reset(self):
rospy.loginfo("HEAD CLIENT: Resetting head.")
self.p.positions = [0, 0]
self.p.velocities = [0, 0]
self.p.time_from_start = rospy.Time(2)
self.set_point()
return self.send_goal()
class Pr2LookAtFace(Action):
def __init__(self):
super(Pr2LookAtFace, self).__init__()
self._client = SimpleActionClient('face_detector_action',FaceDetectorAction)
self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self._timer = None
self._executing = False
self._pending_face_goal = False
self._pending_head_goal = False
def set_duration(self, duration):
duration = max(duration, 1.5)
super(Pr2LookAtFace, self).set_duration(duration)
def to_string(self):
return 'look_at_face()'
def execute(self):
super(Pr2LookAtFace, self).execute()
print('Pr2LookAtFace.execute() %d' % self.get_duration())
# delay execution to not run nested in the current stack context
self._timer = rospy.Timer(rospy.Duration.from_sec(0.001), self._execute, oneshot=True)
def _execute(self, event):
self._executing = True
self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()), self._preempt, oneshot=True)
hgoal = None
connected = self._client.wait_for_server(rospy.Duration(1.0))
if connected:
while self._executing:
fgoal = FaceDetectorGoal()
self._pending_face_goal = True
self._client.send_goal(fgoal)
self._client.wait_for_result()
self._pending_face_goal = False
f = self._client.get_result()
if len(f.face_positions) > 0:
closest = -1
closest_dist = 1000
for i in range(len(f.face_positions)):
dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y\
+ f.face_positions[i].pos.z*f.face_positions[i].pos.z
if dist < closest_dist:
closest = i
closest_dist = dist
if closest > -1:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = f.face_positions[closest].header.frame_id
hgoal.target.point.x = f.face_positions[closest].pos.x
hgoal.target.point.y = f.face_positions[closest].pos.y
hgoal.target.point.z = f.face_positions[closest].pos.z
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
# self._head_client.wait_for_result()
# self._pending_head_goal = False
else:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = "base_footprint";
hgoal.target.point.x = 2.0
hgoal.target.point.y = -2.0
hgoal.target.point.z = 1.2
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
self._head_client.wait_for_result()
self._pending_head_goal = False
if self._executing:
time.sleep(1.0)
self._finished_finding_face()
def _preempt(self, event):
self._executing = False
if self._pending_face_goal:
self._client.cancel_goal()
if self._pending_head_goal:
self._head_client.cancel_goal()
self._execute_finished()
def _finished_finding_face(self):
if self._executing:
self._executing = False
self._timer.shutdown()
self._execute_finished()
class Api(object):
def __init__(self, name):
'''
Wrap the actionlib interface with the API
'''
self._client = SimpleActionClient(name, QueryAction)
rospy.loginfo('waiting for "%s" server', name)
self._client.wait_for_server()
self._feedback = False
self.last_talker_id = ""
def _send_query(self, description, spec, choices):
goal = queryToROS(description, spec, choices)
state = self._client.send_goal(goal, feedback_cb=self._feedback_callback)
def _feedback_callback(self, feedback):
rospy.loginfo("Received feedback")
self._feedback = True
def _wait_for_result_and_get(self, timeout=None):
execute_timeout = rospy.Duration(timeout) if timeout else rospy.Duration(10)
preempt_timeout = rospy.Duration(1)
while not self._client.wait_for_result(execute_timeout):
if not self._feedback:
# preempt action
rospy.logdebug("Canceling goal")
self._client.cancel_goal()
if self._client.wait_for_result(preempt_timeout):
rospy.loginfo("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
else:
rospy.logwarn("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
break
else:
self._feedback = False
rospy.loginfo("I received feedback, let's wait another %.2f seconds" % execute_timeout.to_sec())
state = self._client.get_state()
if state != GoalStatus.SUCCEEDED:
if state == GoalStatus.PREEMPTED:
# Timeout
_print_timeout()
raise TimeoutException("Goal did not succeed within the time limit")
else:
_print_generic_failure()
raise Exception("Goal did not succeed, it was: %s" % GoalStatus.to_string(state))
return self._client.get_result()
def query(self, description, spec, choices, timeout=10):
'''
Perform a HMI query, returns a dict of {choicename: value}
'''
rospy.loginfo('Question: %s, spec: %s', description, _truncate(spec))
_print_example(spec, choices)
self._send_query(description, spec, choices)
answer = self._wait_for_result_and_get(timeout=timeout)
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
_print_answer(answer)
return resultFromROS(answer)
def query_raw(self, description, spec, timeout=10):
'''
Perform a HMI query without choices, returns a string
'''
rospy.loginfo('Question: %s, spec: %s', description, _truncate(spec))
_print_example(spec, {})
self._send_query(description, spec, {})
answer = self._wait_for_result_and_get(timeout=timeout)
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
_print_answer(answer)
return answer.raw_result
def old_query(self, spec, choices, timeout=10):
'''
Convert old queryies to a HMI query
'''
rospy.loginfo('spec: %s', _truncate(spec))
_print_example(spec, choices)
self._send_query('', spec, choices)
try:
answer = self._wait_for_result_and_get(timeout=timeout)
except TimeoutException:
return GetSpeechResponse(result="")
except:
return None
else:
# so we've got an answer
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
_print_answer(answer)
# convert it to the old message
choices = resultFromROS(answer)
result = GetSpeechResponse(result=answer.raw_result)
result.choices = choices
return result
def set_description(self, description):
pass
def set_grammar(self, spec):
pass
def wait_for_grammar_set(self, spec):
pass
class V_search(object):
def __init__(self, height, res):
self.resolution = res # Resolution of the motion
self.motion_height = height # Height of the motion to the ground
# Create search server
self.search_server = SimpleActionServer('search_server',
ExecuteSearchAction,
self.searchCallback, False)
self.server_feedback = ExecuteSearchFeedback()
self.server_result = ExecuteSearchResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
## variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
# Start trajectory server
self.search_server.start()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
if self.search_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def searchCallback(self, search_area):
'''
Execute a search for vitims into the defined area
'''
x = search_area.x # x size of the area to explore
y = search_area.y # y size of the area to explore
start = search_area.origin # Point to start the search
self.next_point.goal.position.x = start.x # Desired x position
self.next_point.goal.position.y = start.y # Desired y position
theta = 0
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
x_positions = ceil(x / self.resolution)
y_positions = ceil(y / self.resolution)
x_count = 0 # Counter of steps (in meters traveled)
direction = 1 # Direction of the motion (right, left or up)
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.search_server.is_preempt_requested():
self.search_server.set_preempted(self.server_result)
return
self.search_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
# Verifies if all the area have been searched
if (self.next_point.goal.position.x == (start.x + x)) and (
(self.next_point.goal.position.y == (start.y + y))):
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
self.search_server.set_succeeded(self.server_result)
return
last_direction = direction
direction = self.square_function(
x_count, 2 * x) # Get the direction of the next step
if last_direction != direction:
# drone moves on y axis
theta = pi / 2
self.next_point.goal.position.y += y / y_positions
elif direction == 1:
# drone moves to the right
theta = 0
self.next_point.goal.position.x += x / x_positions
x_count += x / x_positions
elif direction == -1:
# drone moves to the left
theta = pi
self.next_point.goal.position.x -= x / x_positions
x_count += x / x_positions
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.search_server.set_aborted(self.server_result)
return
def square_function(self, x, max_x):
'''
Function to simulate a square wave function
'''
if round(sin(2 * pi * x / max_x), 2) > 0:
return 1
elif round(sin(2 * pi * x / max_x), 2) < 0:
return -1
else:
if round(cos(2 * pi * x / max_x), 2) > 0:
return 1
else:
return -1
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.release()
class TestMoveActionCancelDrop(unittest.TestCase):
def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def test_cancel_drop_plan_execution(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED or result.error_code.val == 0)
def test_cancel_drop_plan_only(self):
# set the plan only flag
self._goal.planning_options.plan_only = True
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED or result.error_code.val == 0)
def test_cancel_resend(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# send the goal again
self._move_client.send_goal(self._goal)
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
class Client(object):
def __init__(self, name=None, simple_action_client=None):
"""
Wrap the actionlib interface with the API
"""
if not (bool(name) ^ bool(simple_action_client)):
raise ValueError(
'name or simple_action_client should be set, but not both')
if simple_action_client:
self._client = simple_action_client
else:
self._client = SimpleActionClient(name, QueryAction)
rospy.loginfo('waiting for "%s" server', name)
self._client.wait_for_server()
self._feedback = False
self.last_talker_id = ""
def _feedback_callback(self, feedback):
rospy.loginfo("Received feedback")
self._feedback = True
def _wait_for_result_and_get(self, timeout=None):
execute_timeout = rospy.Duration(
timeout) if timeout else rospy.Duration(10)
preempt_timeout = rospy.Duration(1)
while not self._client.wait_for_result(execute_timeout):
if not self._feedback:
# preempt action
rospy.logdebug("Canceling goal")
self._client.cancel_goal()
if self._client.wait_for_result(preempt_timeout):
rospy.loginfo(
"Preempt finished within specified preempt_timeout [%.2f]",
preempt_timeout.to_sec())
else:
rospy.logwarn(
"Preempt didn't finish specified preempt_timeout [%.2f]",
preempt_timeout.to_sec())
break
else:
self._feedback = False
rospy.loginfo(
"I received feedback, let's wait another %.2f seconds" %
execute_timeout.to_sec())
state = self._client.get_state()
if state != GoalStatus.SUCCEEDED:
if state == GoalStatus.PREEMPTED:
# Timeout
_print_timeout()
raise TimeoutException(
"Goal did not succeed within the time limit")
else:
_print_generic_failure()
raise Exception("Goal did not succeed, it was: %s" %
GoalStatus.to_string(state))
return self._client.get_result()
def _send_query(self, description, example_sentences, grammar, target):
goal = QueryGoal(description=description,
example_sentences=example_sentences,
grammar=grammar,
target=target)
self._client.send_goal(goal, feedback_cb=self._feedback_callback)
def query(self, description, grammar, target, timeout=10):
"""
Perform a HMI query, returns a HMIResult
"""
rospy.loginfo('Question: %s, grammar: %s', description,
_truncate(grammar))
# Verify the incoming grammar
verify_grammar(grammar, target)
example_sentences = random_sentences(grammar, target, 10)
_print_example(random.choice(example_sentences))
self._send_query(description, example_sentences, grammar, target)
answer = self._wait_for_result_and_get(timeout=timeout)
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
result = result_from_ros(answer, grammar, target)
_print_result(result)
return result
class Explore(object):
def __init__(self, height):
self.motion_height = height # Height of the motion to the ground
# Create trajectory server
self.exploration_server = SimpleActionServer('assessment_server',
ExecuteAssesstAction,
self.exploreCallback,
False)
self.server_feedback = ExecuteAssesstFeedback()
self.server_result = ExecuteAssesstResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
#Planning scene client
self.frontiers_client = rospy.ServiceProxy('frontiers_server/find',
Frontiers)
self.frontiers_client.wait_for_service()
self.frontiers_req = FrontiersRequest() #Frontiers request message
# Variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
self.scene_req = GetPlanningSceneRequest()
# Start trajectory server
self.exploration_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.notify()
self.odometry_me.release()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
print("\n\n\nASSESSMENT FEEDBACK")
if self.exploration_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def exploreCallback(self, pose):
'''
Execute a loop looking for frontiers and moving to points unvisited into the defined area
'''
# Wait till the robot pose is received
self.odometry_me.acquire()
while self.odometry == None:
self.odometry_me.wait()
self.next_point.goal = self.odometry
self.odometry_me.release()
self.frontiers_req.x_min = 0.0
self.frontiers_req.x_max = 50.0
self.frontiers_req.y_min = 0.0
self.frontiers_req.y_max = 50.0
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.exploration_server.is_preempt_requested():
self.exploration_server.set_preempted(self.server_result)
return
self.exploration_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
p = Pose()
self.odometry_me.acquire()
p = self.odometry
self.frontiers_req.explored.append(p)
self.odometry_me.release()
# Verify if all the area have been explored and find next frontier point if needed
# if 'all area explored':
# self.odometry_me.acquire()
# self.server_result.last_pose = self.odometry
# self.odometry_me.release()
# self.exploration_server.set_succeeded(self.server_result)
status = self.findFrontiers()
if not status:
self.exploration_server.set_succeeded(self.server_result)
return
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
# self.next_point.goal.position.y =
# self.next_point.goal.position.x =
# theta =
# Convert desired angle
# q = quaternion_from_euler(0,0,theta,'ryxz')
# self.next_point.goal.orientation.x = q[0]
# self.next_point.goal.orientation.y = q[1]
# self.next_point.goal.orientation.z = q[2]
# self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.exploration_server.set_aborted(self.server_result)
return
def findFrontiers(self):
'''
Return points not visited into the specified frontier
'''
rospy.loginfo("Looking for frontiers!")
frontiers = self.frontiers_client.call(self.frontiers_req)
if frontiers.frontiers:
self.next_point.goal.position.x = frontiers.frontiers[0].x
self.next_point.goal.position.y = frontiers.frontiers[0].y
self.next_point.goal.position.x = self.motion_height
return True
else:
return False
pick4 = (0.43867932, -1.6539393, 0.03100637, 0.6332176, 1.71315069, 0.9762375, 1.47422282)
place = (0.261, -0.704, 1.470, 0.337, 0.910, -1.667, -0.026)
place2 = (0.8286, -0.7260, -0.3881, 0.6875, -1.2476, 1.5902, 2.0760)
place3 = (0.8254, -0.7281, -0.3977, 0.6896, -1.2834, 1.5953, 2.108)
new_init = (-1.650, -1.465, 3.430, -0.970, -1.427, 0.337, 0.046)
goal_poses = (init,tucked2,)# sample1, sample2, tucked)
num_goals = len(goal_poses)
for k in range(num_goals*1):
goal_pos = goal_poses[k%num_goals]
for i, value in enumerate(goal_pos):
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = value
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.loginfo('Timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('Action finished: %s' % str(state))
else:
rospy.loginfo('Action failed: %s' % str(state))
rospy.sleep(2)
class Head():
def __init__(self, robot_name):
self._robot_name = robot_name
self._ac_head_ref_action = SimpleActionClient("/"+robot_name+"/head_ref/action_server", HeadReferenceAction)
self._goal = None
self._at_setpoint = False
def close(self):
self._ac_head_ref_action.cancel_all_goals()
# -- Helpers --
def reset(self, timeout=0):
"""
Reset head position
"""
reset_goal = PointStamped()
reset_goal.header.stamp = rospy.Time.now()
reset_goal.header.frame_id = "/"+self._robot_name+"/base_link"
reset_goal.point.x = 10
reset_goal.point.y = 0.0
reset_goal.point.z = 0.0
return self.look_at_point(reset_goal, timeout=timeout)
def look_at_hand(self, side):
"""
Look at the left or right hand, expects string "left" or "right"
Optionally, keep tracking can be disabled (keep_tracking=False)
"""
if side == "left":
return self.look_at_point(msgs.PointStamped(0,0,0,frame_id="/"+self._robot_name+"/grippoint_left"))
elif side == "right":
return self.look_at_point(msgs.PointStamped(0,0,0,frame_id="/"+self._robot_name+"/grippoint_right"))
else:
rospy.logerr("No side specified for look_at_hand. Give me 'left' or 'right'")
return False
def look_at_ground_in_front_of_robot(self, distance=2):
goal = PointStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "/"+self._robot_name+"/base_link"
goal.point.x = distance
goal.point.y = 0.0
goal.point.z = 0.0
return self.look_at_point(goal)
def look_down(self, timeout=0):
"""
Gives a target at z = 1.0 at 1 m in front of the robot
"""
goal = PointStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "/"+self._robot_name+"/base_link"
goal.point.x = 1.0
goal.point.y = 0.0
goal.point.z = 0.5
return self.look_at_point(goal)
def look_up(self, timeout=0):
"""
Gives a target at z = 1.0 at 1 m in front of the robot
"""
goal = PointStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "/"+self._robot_name+"/base_link"
goal.point.x = 0.2
goal.point.y = 0.0
goal.point.z = 4.5
return self.look_at_point(goal)
def look_at_standing_person(self, timeout=0):
"""
Gives a target at z = 1.75 at 1 m in front of the robot
"""
goal = PointStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "/"+self._robot_name+"/base_link"
goal.point.x = 1.0
goal.point.y = 0.0
goal.point.z = 1.6
return self.look_at_point(goal)
# -- Functionality --
def look_at_point(self, point_stamped, end_time=0, pan_vel=1.0, tilt_vel=1.0, timeout=0):
self._setHeadReferenceGoal(0, pan_vel, tilt_vel, end_time, point_stamped, timeout=timeout)
def cancel_goal(self):
self._ac_head_ref_action.cancel_goal()
self._goal = None
self._at_setpoint = False
def wait_for_motion_done(self, timeout=5.0):
self._at_setpoint = False
starttime = rospy.Time.now()
if self._goal:
while (rospy.Time.now() - starttime).to_sec() < timeout:
if self._at_setpoint:
rospy.sleep(0.3)
return True
else:
rospy.sleep(0.1)
return False
# ---- INTERFACING THE NODE ---
def _setHeadReferenceGoal(self, goal_type, pan_vel, tilt_vel, end_time, point_stamped=PointStamped(), pan=0, tilt=0, timeout=0):
self.cancel_goal()
self._goal = HeadReferenceGoal()
self._goal.goal_type = goal_type
self._goal.priority = 0 # Executives get prio 1
self._goal.pan_vel = pan_vel
self._goal.tilt_vel = tilt_vel
self._goal.target_point = point_stamped
self._goal.pan = pan
self._goal.tilt = tilt
self._goal.end_time = end_time
self._ac_head_ref_action.send_goal(self._goal, done_cb = self.__doneCallback, feedback_cb = self.__feedbackCallback)
start = rospy.Time.now()
if timeout != 0:
print "Waiting for %d seconds to reach target ..."%timeout
while (rospy.Time.now() - start) < rospy.Duration(timeout) and not self._at_setpoint:
rospy.sleep(0.1)
def __feedbackCallback(self, feedback):
self._at_setpoint = feedback.at_setpoint
def __doneCallback(self, terminal_state, result):
self._goal = None
self._at_setpoint = False
class StateMachine(object):
def __init__(self):
self.node_name = "Student SM"
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
#self.amcl_estimate = rospy.get_param(rospy.get_name() + '/amcl_estimate')
self.aruco_pose_topic = rospy.get_param(rospy.get_name() +
'/aruco_pose_topic')
#self.clear_costmaps_srv = rospy.get_param(rospy.get_name() + '/clear_costmaps_srv')
self.cube_pos = rospy.get_param(rospy.get_name() + '/cube_pose')
#self.global_loc_srv = rospy.get_param(rospy.get_name() + '/global_loc_srv')
#self.move_base_feedback = rospy.get_param(rospy.get_name() + '/move_base_feedback')
#self.move_base_frame = rospy.get_param(rospy.get_name() + '/move_base_frame')
#self.nav_goal_topic = rospy.get_param(rospy.get_name() + '/nav_goal_topic')
#self.pick_pose_topic = rospy.get_param(rospy.get_name() + '/pick_pose_topic')
self.pick_srv = rospy.get_param(rospy.get_name() + '/pick_srv')
#self.place_pose_topic = rospy.get_param(rospy.get_name() + '/place_pose_topic')
self.place_srv = rospy.get_param(rospy.get_name() + '/place_srv')
#self.robot_base_frame = rospy.get_param(rospy.get_name() + '/robot_base_frame')
# Subscribe to topics
# ---
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
rospy.wait_for_service(self.pick_srv, timeout=30)
rospy.wait_for_service(self.place_srv, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
self.aruco_pose_pub = rospy.Publisher(self.aruco_pose_topic,
PoseStamped,
queue_size=10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
# Init state machine
self.state = 0
rospy.sleep(3)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state != 4:
# State 0: Get in safe position
if self.state == 0:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
success_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(100.0))
if success_tucking:
rospy.loginfo("%s: Arm tucked.", self.node_name)
self.state = 1
else:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = 5
rospy.sleep(1)
# State 1: Pick up the cube
if self.state == 1:
rospy.loginfo("%s: Picking up the cube...", self.node_name)
try:
self.cube_pos = self.cube_pos.split(',')
pick_pose_msg = PoseStamped()
pick_pose_msg.header.frame_id = "base_footprint"
pick_pose_msg.pose.position.x = float(
self.cube_pos[0]) + 0.03
pick_pose_msg.pose.position.y = float(self.cube_pos[1])
pick_pose_msg.pose.position.z = float(
self.cube_pos[2]) - 0.06
pick_pose_msg.pose.orientation.x = float(self.cube_pos[3])
pick_pose_msg.pose.orientation.y = float(self.cube_pos[4])
pick_pose_msg.pose.orientation.z = float(self.cube_pos[5])
pick_pose_msg.pose.orientation.w = float(self.cube_pos[6])
self.aruco_pose_pub.publish(pick_pose_msg)
pick_srv = rospy.ServiceProxy(self.pick_srv, SetBool)
pick_req = pick_srv(True)
if pick_req.success == True:
rospy.loginfo("%s: Cube picked.", self.node_name)
self.state = 2
else:
rospy.loginfo("%s: Failed to pick cube.",
self.node_name)
self.state = 5
rospy.sleep(3) #it could also be 1
except rospy.ServiceException, e:
print "Service call to pick_srv server failed: %s" % e
# State 2: Move the robot "manually" to the table
if self.state == 2:
move_msg = Twist()
move_msg.angular.z = -1
rate = rospy.Rate(10)
converged = False
cnt = 0
rospy.loginfo("%s: Moving towards table", self.node_name)
while not rospy.is_shutdown() and cnt < 30:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
move_msg.linear.x = 1
move_msg.angular.z = 0
cnt = 0
while not rospy.is_shutdown() and cnt < 9:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 3
rospy.sleep(1)
# State 3: Place the cube
if self.state == 3:
rospy.loginfo("%s: Placing the cube...", self.node_name)
try:
place_pose_msg = PoseStamped()
place_pose_msg.header.frame_id = "base_footprint"
place_pose_msg.pose.position.x = float(
self.cube_pos[0]) + 0.03
place_pose_msg.pose.position.y = float(self.cube_pos[1])
place_pose_msg.pose.position.z = float(
self.cube_pos[2]) - 0.06
place_pose_msg.pose.orientation.x = float(self.cube_pos[3])
place_pose_msg.pose.orientation.y = float(self.cube_pos[4])
place_pose_msg.pose.orientation.z = float(self.cube_pos[5])
place_pose_msg.pose.orientation.w = float(self.cube_pos[6])
self.aruco_pose_pub.publish(place_pose_msg)
place_srv = rospy.ServiceProxy(self.place_srv, SetBool)
place_req = place_srv(True)
if place_req.success == True:
rospy.loginfo("%s: Cube placed.", self.node_name)
self.state = 4
else:
rospy.loginfo("%s: Failed to place cube.",
self.node_name)
self.state = 5
rospy.sleep(3) #it could also be 1
except rospy.ServiceException, e:
print "Service call to place_srv server failed: %s" % e
# # State 3: Lower robot head service
# if self.state == 3:
# try:
# rospy.loginfo("%s: Lowering robot head", self.node_name)
# move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm, MoveHead)
# move_head_req = move_head_srv("down")
# if move_head_req.success == True:
# self.state = 4
# rospy.loginfo("%s: Move head down succeded!", self.node_name)
# else:
# rospy.loginfo("%s: Move head down failed!", self.node_name)
# self.state = 5
# rospy.sleep(3)
# except rospy.ServiceException, e:
# print "Service call to move_head server failed: %s"%e
# Error handling
if self.state == 5:
rospy.logerr(
"%s: State machine failed. Check your code and try again!",
self.node_name)
return
class GiskardWrapper(object):
def __init__(self, giskard_topic=u'qp_controller/command', ns=u'giskard'):
if giskard_topic is not None:
self.client = SimpleActionClient(giskard_topic, MoveAction)
self.update_world = rospy.ServiceProxy(
u'{}/update_world'.format(ns), UpdateWorld)
# self.marker_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(ns))
self.client.wait_for_server()
self.tip_to_root = {}
self.collisions = []
self.clear_cmds()
self.object_js_topics = {}
rospy.sleep(.3)
def set_cart_goal(self, root, tip, pose_stamped):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
self.set_tranlation_goal(root, tip, pose_stamped)
self.set_rotation_goal(root, tip, pose_stamped)
def set_tranlation_goal(self, root, tip, pose_stamped):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
controller = Controller()
controller.root_link = str(root)
controller.tip_link = str(tip)
controller.goal_pose = pose_stamped
controller.type = Controller.TRANSLATION_3D
controller.weight = 1
controller.max_speed = 0.3
controller.p_gain = 3
self.cmd_seq[-1].controllers.append(controller)
def set_rotation_goal(self, root, tip, pose_stamped):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
controller = Controller()
controller.root_link = str(root)
controller.tip_link = str(tip)
controller.goal_pose = pose_stamped
controller.type = Controller.ROTATION_3D
controller.weight = 1
controller.max_speed = 1.0
controller.p_gain = 3
self.cmd_seq[-1].controllers.append(controller)
def set_joint_goal(self, joint_state):
"""
:param joint_state:
:type joint_state: dict
"""
controller = Controller()
controller.type = Controller.JOINT
controller.weight = 1
controller.p_gain = 10
controller.max_speed = 1
if isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
controller.goal_state.name.append(joint_name)
controller.goal_state.position.append(joint_position)
elif isinstance(joint_state, JointState):
controller.goal_state = joint_state
self.cmd_seq[-1].controllers.append(controller)
def set_collision_entries(self, collisions):
self.cmd_seq[-1].collisions.extend(collisions)
def avoid_collision(self,
min_dist,
robot_link=u'',
body_b=u'',
link_b=u''):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_link = str(robot_link)
collision_entry.body_b = str(body_b)
collision_entry.link_b = str(link_b)
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_ALL_COLLISIONS
self.set_collision_entries([collision_entry])
def add_cmd(self, max_trajectory_length=20):
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = max_trajectory_length
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:return:
:rtype: giskard_msgs.msg._MoveResult.MoveResult
"""
goal = self._get_goal()
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
self.client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
self.client.wait_for_result(timeout)
return self.client.get_result()
def clear_world(self):
"""
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(),
False, PoseStamped())
return self.update_world.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False,
PoseStamped())
return self.update_world.call(req)
def make_box(self, name=u'box', size=(1, 1, 1)):
box = WorldBody()
box.type = WorldBody.PRIMITIVE_BODY
box.name = str(name)
box.shape.type = SolidPrimitive.BOX
box.shape.dimensions.append(size[0])
box.shape.dimensions.append(size[1])
box.shape.dimensions.append(size[2])
return box
def add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1)):
box = self.make_box(name, size)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self.update_world.call(req)
def add_sphere(self,
name=u'sphere',
size=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1)):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_cylinder(self,
name=u'cylinder',
size=(1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1)):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions.append(size[0])
object.shape.dimensions.append(size[1])
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def attach_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1)):
"""
:param name:
:param size:
:param frame_id:
:param position:
:param orientation:
:rtype: UpdateWorldResponse
"""
box = self.make_box(name, size)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self.update_world.call(req)
def add_urdf(self, name, urdf, js_topic, pose):
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
self.object_js_topics[name] = rospy.Publisher(js_topic,
JointState,
queue_size=10)
return self.update_world.call(req)
def set_object_joint_state(self, object_name, joint_states):
if isinstance(joint_states, dict):
joint_states = dict_to_joint_states(joint_states)
self.object_js_topics[object_name].publish(joint_states)
#request a vertical lift of 10cm after grasping the object
pickup_goal.lift.desired_distance = 0.1
#do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = 0
pickup_goal.use_reactive_execution = 0
#send the goal
pickup_client.send_goal(pickup_goal)
rospy.loginfo("sent pickup goal")
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = pickup_client.wait_for_result(rospy.Duration(30))
if not finished_within_time:
pickup_client.cancel_goal()
rospy.loginfo("Timed out achieving pickup goal")
return False
else:
state = pickup_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Pickup goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Pickup goal failed with error code: " + str(state))
return False
return True # Check for success or failure
else:
return False
class ArmCommander(Limb):
"""
This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages
Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation
"""
def __init__(self,
name,
rate=100,
fk='robot',
ik='trac',
default_kv_max=1.,
default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {
'fk': {
'ros': 'compute_fk'
},
'ik': {
'ros':
'compute_ik',
'robot':
'ExternalTools/{}/PositionKinematicsNode/IKService'.format(
name),
'trac':
'trac_ik_{}'.format(name)
}
}
self._kinematics_services = {
'fk': {
'ros': {
'service':
rospy.ServiceProxy(self._kinematics_names['fk']['ros'],
GetPositionFK),
'func':
self._get_fk_ros
},
'kdl': {
'func': self._get_fk_pykdl
},
'robot': {
'func': self._get_fk_robot
}
},
'ik': {
'ros': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['ros'],
GetPositionIK),
'func':
self._get_ik_ros
},
'robot': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['robot'],
SolvePositionIK),
'func':
self._get_ik_robot
},
'trac': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['trac'],
GetConstrainedPositionIK),
'func':
self._get_ik_trac
},
'kdl': {
'func': self._get_ik_pykdl
}
}
}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(
self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(
self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber(
'/robot/limb/{}/collision_detection_state'.format(name),
CollisionDetectionState,
self._cb_collision,
queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name),
DigitalIOState,
self._cb_dig_io,
queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(
self.name)
self.client = SimpleActionClient(action_server_name,
FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher(
"/move_group/display_planned_path",
DisplayTrajectory,
queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
######################################### CALLBACKS #########################################
def _cb_collision(self, msg):
if msg.collision_state:
with self._stop_lock:
self._stop_reason = 'collision'
def _cb_dig_io(self, msg):
if msg.state > 0:
with self._stop_lock:
self._stop_reason = 'cuff'
#############################################################################################
def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
]]
def endpoint_name(self):
return self.name + '_gripper'
def group_name(self):
return self.name + '_arm'
def joint_limits(self):
xml_urdf = rospy.get_param('robot_description')
dict_urdf = xmltodict.parse(xml_urdf)
joints_urdf = []
joints_urdf.append([
j['@name'] for j in dict_urdf['robot']['joint']
if j['@name'] in self.joint_names()
])
joints_urdf.append(
[[float(j['limit']['@lower']),
float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint']
if j['@name'] in self.joint_names()])
# reorder the joints limits
return dict(
zip(self.joint_names(), [
joints_urdf[1][joints_urdf[0].index(name)]
for name in self.joint_names()
]))
def get_current_state(self, list_joint_names=[]):
"""
Returns the current RobotState describing all joint states
:param list_joint_names: If not empty, returns only the state of the requested joints
:return: a RobotState corresponding to the current state read on /robot/joint_states
"""
if len(list_joint_names) == 0:
list_joint_names = self.joint_names()
state = RobotState()
state.joint_state.name = list_joint_names
state.joint_state.position = map(self.joint_angle, list_joint_names)
state.joint_state.velocity = map(self.joint_velocity, list_joint_names)
state.joint_state.effort = map(self.joint_effort, list_joint_names)
return state
def get_ik(self, eef_poses, seeds=(), source=None, params=None):
"""
Return IK solutions of this arm's end effector according to the method declared in the constructor
:param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped
:param seeds: a single seed or a list of seeds of type RobotState for each input pose
:param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated)
:param params: dictionary containing optional non-generic IK parameters
:return: a list of RobotState for each input pose in input or a single RobotState
TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None)
Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose
"""
if not isinstance(eef_poses, list) or isinstance(
eef_poses[0], list) and not isinstance(eef_poses[0][0], list):
eef_poses = [eef_poses]
if not seeds:
seeds = []
elif not isinstance(seeds, list):
seeds = [seeds] * len(eef_poses)
input = []
for eef_pose in eef_poses:
if isinstance(eef_pose, list):
input.append(list_to_pose(eef_pose, self._world))
elif isinstance(eef_pose, PoseStamped):
input.append(eef_pose)
else:
raise TypeError(
"ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}"
.format(str(type(eef_pose))))
output = self._kinematics_services['ik'][
self._selected_ik if source is None else source]['func'](input,
seeds,
params)
return output if len(eef_poses) > 1 else output[0]
def get_fk(self, frame_id=None, robot_state=None):
"""
Return The FK solution oth this robot state according to the method declared in the constructor
robot_state = None will give the current endpoint pose in frame_id
:param robot_state: a RobotState message
:param frame_id: the frame you want the endpoint pose into
:return: [[x, y, z], [x, y, z, w]]
"""
if frame_id is None:
frame_id = self._world
if isinstance(robot_state, RobotState) or robot_state is None:
return self._kinematics_services['fk'][self._selected_fk]['func'](
frame_id, robot_state)
else:
raise TypeError(
"ArmCommander.get_fk() accepts only a RobotState, got {}".
format(str(type(robot_state))))
def _get_fk_pykdl(self, frame_id, state=None):
if state is None:
state = self.get_current_state()
fk = self._kinematics_pykdl.forward_position_kinematics(
dict(zip(state.joint_state.name, state.joint_state.position)))
ps = list_to_pose([fk[:3], fk[-4:]], self._world)
return self._tf_listener.transformPose(frame_id, ps)
def _get_fk_robot(self, frame_id=None, state=None):
# Keep this half-working FK, still used by generate_cartesian_path (trajectories.py)
if state is not None:
raise NotImplementedError(
"_get_fk_robot has no FK service provided by the robot except for its current endpoint pose"
)
ps = list_to_pose(self.endpoint_pose(), self._world)
return self._tf_listener.transformPose(frame_id, ps)
def _get_fk_ros(self, frame_id=None, state=None):
rqst = GetPositionFKRequest()
rqst.header.frame_id = self._world if frame_id is None else frame_id
rqst.fk_link_names = [self.endpoint_name()]
if isinstance(state, RobotState):
rqst.robot_state = state
elif isinstance(state, JointState):
rqst.robot_state.joint_state = state
elif state is None:
rqst.robot_state = self.get_current_state()
else:
raise AttributeError("Provided state is an invalid type")
fk_answer = self._kinematics_services['fk']['ros']['service'].call(
rqst)
if fk_answer.error_code.val == 1:
return fk_answer.pose_stamped[0]
else:
return None
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError(
"_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}"
.format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(
pose[0], pose[1], [
seeds[pose_num].joint_state.position[
seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()
] if len(seeds) > 0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def _get_ik_robot(self, eef_poses, seeds=(), params=None):
ik_req = SolvePositionIKRequest()
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
ik_req.seed_mode = ik_req.SEED_USER if len(
seeds) > 0 else ik_req.SEED_CURRENT
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['robot']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(
RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_trac(self, eef_poses, seeds=(), params=None):
ik_req = GetConstrainedPositionIKRequest()
if params is None:
ik_req.num_steps = 1
else:
ik_req.end_tolerance = params['end_tolerance']
ik_req.num_steps = params['num_attempts']
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
if len(seeds) == 0:
seeds = [self.get_current_state()] * len(eef_poses)
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['trac']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(
RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_ros(self, eef_poses, seeds=()):
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = self.group_name()
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
rqst.ik_request.pose_stamped = eef_pose # Do we really to do a separate call for each pose? _vector not used
ik_answer = self._kinematics_services['ik']['ros']['service'].call(
rqst)
if len(seeds) > 0:
rqst.ik_request.robot_state = seeds[pose_num]
if ik_answer.error_code.val == 1:
# Apply a filter to return only joints of this group
try:
ik_answer.solution.joint_state.velocity = [
value for id_joint, value in enumerate(
ik_answer.solution.joint_state.velocity)
if ik_answer.solution.joint_state.name[id_joint] in
self.joint_names()
]
ik_answer.solution.joint_state.effort = [
value for id_joint, value in enumerate(
ik_answer.solution.joint_state.effort)
if ik_answer.solution.joint_state.name[id_joint] in
self.joint_names()
]
except IndexError:
pass
ik_answer.solution.joint_state.position = [
value for id_joint, value in enumerate(
ik_answer.solution.joint_state.position)
if ik_answer.solution.joint_state.name[id_joint] in
self.joint_names()
]
ik_answer.solution.joint_state.name = [
joint for joint in ik_answer.solution.joint_state.name
if joint in self.joint_names()
]
solutions.append(ik_answer.solution)
else:
solutions.append(None)
return solutions
def translate_to_cartesian(self,
path,
frame_id,
time,
n_points=50,
max_speed=np.pi / 4,
min_success_rate=0.5,
display_only=False,
timeout=0,
stop_test=lambda: False,
pause_test=lambda: False):
"""
Translate the end effector in straight line, following path=[translate_x, translate_y, translate_z] wrt frame_id
:param path: Path [x, y, z] to follow wrt frame_id
:param frame_id: frame_id of the given input path
:param time: Time of the generated trajectory
:param n_points: Number of 3D points of the cartesian trajectory
:param max_speed: Maximum speed for every single joint in rad.s-1, allowing to avoid jumps in joints configuration
:param min_success_rate: Raise RuntimeError in case the success rate is lower than min_success_rate
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up (default is 0 = do not retry)
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return:
:raises: RuntimeError if success rate is too low
"""
def trajectory_callable(start):
traj, success_rate = trajectories.generate_cartesian_path(
path, frame_id, time, self, None, n_points, max_speed)
if success_rate < min_success_rate:
raise RuntimeError(
"Unable to generate cartesian path (success rate : {})".
format(success_rate))
return traj
return self._relaunched_move_to(trajectory_callable, display_only,
timeout, stop_test, pause_test)
def move_to_controlled(self,
goal,
rpy=[0, 0, 0],
display_only=False,
timeout=15,
stop_test=lambda: False,
pause_test=lambda: False):
"""
Move to a goal using interpolation in joint space with limitation of velocity and acceleration
:param goal: Pose, PoseStamped or RobotState
:param rpy: Vector [Roll, Pitch, Yaw] filled with 0 if this axis must be preserved, 1 otherwise
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return: None
:raises: ValueError if IK has no solution
"""
assert callable(stop_test)
assert callable(pause_test)
if not isinstance(goal, RobotState):
goal = self.get_ik(goal)
if goal is None:
raise ValueError('This goal is not reachable')
# collect the robot state
start = self.get_current_state()
# correct the orientation if rpy is set
if np.array(rpy).any():
# convert the starting point to rpy pose
pos, rot = states.state_to_pose(start, self, True)
for i in range(3):
if rpy[i]:
rpy[i] = rot[i]
goal = states.correct_state_orientation(goal, rpy, self)
# parameters for trapezoidal method
kv_max = self.kv_max
ka_max = self.ka_max
return self._relaunched_move_to(
lambda start: trajectories.trapezoidal_speed_trajectory(
goal, start=start, kv_max=kv_max, ka_max=ka_max), display_only,
timeout, stop_test, pause_test)
def rotate_joint(self,
joint_name,
goal_angle,
display_only=False,
stop_test=lambda: False,
pause_test=lambda: False):
goal = self.get_current_state()
joint = goal.joint_state.name.index(joint_name)
# JTAS accepts all angles even out of limits
#limits = self.joint_limits()[joint_name]
goal.joint_state.position[joint] = goal_angle
return self.move_to_controlled(goal,
display_only=display_only,
stop_test=stop_test,
pause_test=pause_test)
def _relaunched_move_to(self,
trajectory_callable,
display_only=False,
timeout=15,
stop_test=lambda: False,
pause_test=lambda: False):
"""
Relaunch several times (until cuff interaction or failure) a move_to() whose trajectory is generated by the callable passed in parameter
:param trajectory_callable: Callable to call to recompute the trajectory
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
:return:
"""
assert callable(trajectory_callable)
retry = True
t0 = rospy.get_time()
while retry and rospy.get_time() - t0 < timeout or timeout == 0:
start = self.get_current_state()
trajectory = trajectory_callable(start)
if display_only:
self.display(trajectory)
break
else:
retry = not self.execute(
trajectory, test=lambda: stop_test() or pause_test())
if pause_test():
if not stop_test():
retry = True
while pause_test():
rospy.sleep(0.1)
if timeout == 0:
return not display_only and not retry
if retry:
rospy.sleep(1)
return not display_only and not retry
def get_random_pose(self):
# get joint names
joint_names = self.joint_names()
# create a random joint state
bounds = []
for key, value in self.joint_limits().iteritems():
bounds.append(value)
bounds = np.array(bounds)
joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1],
len(joint_names))
# append it in a robot state
goal = RobotState()
goal.joint_state.name = joint_names
goal.joint_state.position = joint_state
goal.joint_state.header.stamp = rospy.Time.now()
goal.joint_state.header.frame_id = 'base'
return goal
######################## OPERATIONS ON TRAJECTORIES
# TO BE MOVED IN trajectories.py
def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time * (1.0 / nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j], goal_state[j],
nb_points + 1)
joints.append(pose_lin[1:].tolist())
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i + 1) * dt)
# append the position to the message
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(
JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError(
"ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}"
.format(str(type(trajectory))))
self._display_traj.publish(dt)
def execute(self, trajectory, test=None, epsilon=0.1):
"""
Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
Non-blocking needs should deal with the JointTrajectory action server
:param trajectory: either a RobotTrajectory or a JointTrajectory
:param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
:return: True if execution ended successfully, False otherwise
"""
def distance_to_first_point(point):
joint_pos = np.array(point.positions)
return np.linalg.norm(current_array - joint_pos)
self.display(trajectory)
with self._stop_lock:
self._stop_reason = ''
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
elif not isinstance(trajectory, JointTrajectory):
raise TypeError(
"Execute only accept RobotTrajectory or JointTrajectory")
ftg = FollowJointTrajectoryGoal()
ftg.trajectory = trajectory
# check if it is necessary to move to the first point
current = self.get_current_state()
current_array = np.array([
current.joint_state.position[current.joint_state.name.index(joint)]
for joint in trajectory.joint_names
])
if distance_to_first_point(trajectory.points[0]) > epsilon:
# convert first point to robot state
rs = RobotState()
rs.joint_state.name = trajectory.joint_names
rs.joint_state.position = trajectory.points[0].positions
# move to the first point
self.move_to_controlled(rs)
# execute the input trajectory
self.client.send_goal(ftg)
# Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory
while self.client.simple_state != SimpleGoalState.DONE:
if callable(test) and test():
self.client.cancel_goal()
return True
if self._stop_reason != '':
self.client.cancel_goal()
return False
self._rate.sleep()
return True
def close(self):
"""
Open the gripper
:return: True if an object has been grasped
"""
return self._gripper.close(True)
def open(self):
"""
Close the gripper
return: True if an object has been released
"""
return self._gripper.open(True)
def gripping(self):
return self._gripper.gripping()
def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True):
"""
Blocks until external motion is given to the arm
:param threshold:
:param rate: rate of control loop in Hertz
:param ignore_gripping: True if we must wait even if no object is gripped
"""
def detect_variation():
new_effort = np.array(
self.get_current_state(
[self.name + '_w0', self.name + '_w1',
self.name + '_w2']).joint_state.effort)
delta = np.absolute(effort - new_effort)
return np.amax(delta) > threshold
# record the effort at calling time
effort = np.array(
self.get_current_state(
[self.name + '_w0', self.name + '_w1',
self.name + '_w2']).joint_state.effort)
# loop till the detection of changing effort
rate = rospy.Rate(rate)
while not detect_variation() and not rospy.is_shutdown() and (
ignore_gripping or self.gripping()):
rate.sleep()
class ErraticBaseActionServer():
def __init__(self):
self.base_frame = '/base_footprint'
self.move_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.result = ErraticBaseResult()
self.feedback = ErraticBaseFeedback()
self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
self.server.start()
rospy.loginfo("%s: Ready to accept goals", NAME)
def transform_target_point(self, point):
self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
return self.tf.transformPoint(self.base_frame, point)
def move_to(self, target_pose):
goal = MoveBaseGoal()
goal.target_pose = target_pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb)
while not self.move_client.wait_for_result(rospy.Duration(0.01)):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
def get_vicinity_target(self, target_pose, vicinity_range):
vicinity_pose = PoseStamped()
# transform target to base_frame reference
target_point = PointStamped()
target_point.header.frame_id = target_pose.header.frame_id
target_point.point = target_pose.pose.position
self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0))
target = self.tf.transformPoint(self.base_frame, target_point)
rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z)
# find distance to point
dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
if (dist < vicinity_range):
# if already within range, then no need to move
vicinity_pose.pose.position.x = 0.0
vicinity_pose.pose.position.y = 0.0
else:
# normalize vector pointing from source to target
target.point.x /= dist
target.point.y /= dist
# scale normal vector to within vicinity_range distance from target
target.point.x *= (dist - vicinity_range)
target.point.y *= (dist - vicinity_range)
# add scaled vector to source
vicinity_pose.pose.position.x = target.point.x + 0.0
vicinity_pose.pose.position.y = target.point.y + 0.0
# set orientation
ori = Quaternion()
yaw = math.atan2(target.point.y, target.point.x)
(ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
vicinity_pose.pose.orientation = ori
# prep header
vicinity_pose.header = target_pose.header
vicinity_pose.header.frame_id = self.base_frame
rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z)
return vicinity_pose
def execute_callback(self, goal):
rospy.loginfo("%s: Executing move base", NAME)
move_base_result = None
if goal.vicinity_range == 0.0:
# go to exactly
move_base_result = self.move_to(goal.target_pose)
else:
# go near (within vicinity_range meters)
vicinity_target_pose = self.get_vicinity_target(goal.target_pose, goal.vicinity_range)
move_base_result = self.move_to(vicinity_target_pose)
# check results
if (move_base_result == GoalStatus.SUCCEEDED):
rospy.loginfo("%s: Succeeded", NAME)
self.result.base_position = self.feedback.base_position
self.server.set_succeeded(self.result)
elif (move_base_result == GoalStatus.PREEMPTED):
rospy.loginfo("%s: Preempted", NAME)
self.server.set_preempted()
else:
rospy.loginfo("%s: Aborted", NAME)
self.server.set_aborted()
new_init = (-1.650, -1.465, 3.430, -0.970, -1.427, 0.337, 0.046)
goal_poses = (
init,
tucked2,
) # sample1, sample2, tucked)
num_goals = len(goal_poses)
for k in range(num_goals * 1):
goal_pos = goal_poses[k % num_goals]
for i, value in enumerate(goal_pos):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = value
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(
rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.loginfo('Timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('Action finished: %s' % str(state))
else:
rospy.loginfo('Action failed: %s' % str(state))
rospy.sleep(2)
class PickUpActionServer():
def __init__(self):
# Initialize new node
rospy.init_node(NAME)#, anonymous=False)
#initialize the clients to interface with
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.move_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
# Initialize tf listener (remove?)
self.tf = tf.TransformListener()
# Initialize erratic base action server
self.result = SmartArmGraspResult()
self.feedback = SmartArmGraspFeedback()
self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)
#define the offsets
# These offsets were determines expirmentally using the simulator
# They were tested using points stamped with /map
self.xOffset = 0.025
self.yOffset = 0.0
self.zOffset = 0.12 #.05 # this does work!
rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def move_to(self, frame_id, position, orientation, vicinity=0.0):
goal = ErraticBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = frame_id
goal.target_pose.pose.position = position
goal.target_pose.pose.orientation = orientation
goal.vicinity_range = vicinity
self.move_client.send_goal(goal)
#print "going into loop"
while (not self.move_client.wait_for_result(rospy.Duration(0.01))):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
#(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position
def transform_target_point(self, point, frameId):
#rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(frameId, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point & retrieve the pan angle
return self.tf.transformPoint(frameId, point)
#UNUSED
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
goal = SmartArmGoal()
goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
# This moves the wrist of the arm to the x, y, z coordinates
def reach_at(self, frame_id, x, y, z):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = frame_id
goal.target_point.point.x = x
goal.target_point.point.y = y
goal.target_point.point.z = z
rospy.loginfo("%s: Original point is '%s' [%f, %f, %f]", NAME, frame_id,\
goal.target_point.point.x, goal.target_point.point.y, goal.target_point.point.z)
goal.target_point = self.transform_target_point(goal.target_point, '/arm_base_link');
rospy.loginfo("%s: Transformed point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
goal.target_point.point.x = goal.target_point.point.x + self.xOffset
goal.target_point.point.y = goal.target_point.point.y + self.yOffset
goal.target_point.point.z = goal.target_point.point.z + self.zOffset
rospy.loginfo("%s: Transformed and Offset point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def arm_position_feedback_cb(self, fb):
self.feedback.arm_position = fb.arm_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This moves the gripper to the given angles
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal, None, None, self.gripper_position_feedback_cb)
#gripper_client.wait_for_goal_to_finish()
while not self.gripper_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.gripper_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.gripper_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def gripper_position_feedback_cb(self, fb):
self.feedback.gripper_position = fb.gripper_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This method sets the results of the goal to the last feedback values
def set_results(self, success):
self.result.success = success
self.result.arm_position = self.feedback.arm_position
self.result.gripper_position = self.feedback.gripper_position
self.server.set_succeeded(self.result)
#This is the callback function that is executed whenever the server
#recieves a request
def execute_callback(self, goal):
rospy.loginfo("%s: Executing Grasp Action [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
position = Point(x = goal.target_point.point.x, y = goal.target_point.point.y)
orientation = Quaternion(w=1.0)
self.move_to('/map', position, orientation, 0.5)
self.move_to('/map', position, orientation, 0.2)
rospy.sleep(0.2)
rospy.loginfo( "%s: Opening gripper", NAME)
#open the gripper
result = self.move_gripper(0.2, -0.2)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Open Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Open Gripper Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the arm to the object", NAME)
#move the arm to the correct posisions
result = self.reach_at(goal.target_point.header.frame_id, \
goal.target_point.point.x, \
goal.target_point.point.y, \
goal.target_point.point.z)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 2nd Move Arm (to object) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 2nd Move Arm (to object) Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the elbow joint to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(self.feedback.arm_position[0], self.feedback.arm_position[1], \
-3.14 / 2 - self.feedback.arm_position[1], self.feedback.arm_position[3])
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Moving the elbow joint Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Moving the elbow joint Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Closing gripper", NAME)
#close the gripper
result = self.move_gripper(-1.5, 1.5)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Close Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
#this actions 'succeeds' if it times out
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
#action has succeeded
rospy.loginfo("%s: Grasp Action Succeed [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.set_results(True)
"""
class SimpleExerciser(unittest.TestCase):
def setUp(self):
self.default_wait = rospy.Duration(60.0)
self.client = SimpleActionClient('test_request_action', TestRequestAction)
self.assert_(self.client.wait_for_server(self.default_wait))
def test_just_succeed(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_abort(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_ABORTED,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_preempt(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate = rospy.Duration(100000),
the_result = 42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
# Should print out errors about not setting a terminal status in the action server.
def test_drop(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_DROP,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
# Should print out errors about throwing an exception
def test_exception(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_EXCEPTION,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
def test_ignore_cancel_and_succeed(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate = rospy.Duration(2.0),
ignore_cancel = True,
the_result = 42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_lose(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_LOSE,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.LOST, self.client.get_state())
class Pr2LookAtFace(Action):
def __init__(self):
super(Pr2LookAtFace, self).__init__()
self._client = SimpleActionClient('face_detector_action',
FaceDetectorAction)
self._head_client = SimpleActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
self._timer = None
self._executing = False
self._pending_face_goal = False
self._pending_head_goal = False
def set_duration(self, duration):
duration = max(duration, 1.5)
super(Pr2LookAtFace, self).set_duration(duration)
def to_string(self):
return 'look_at_face()'
def execute(self):
super(Pr2LookAtFace, self).execute()
print('Pr2LookAtFace.execute() %d' % self.get_duration())
# delay execution to not run nested in the current stack context
self._timer = rospy.Timer(rospy.Duration.from_sec(0.001),
self._execute,
oneshot=True)
def _execute(self, event):
self._executing = True
self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()),
self._preempt,
oneshot=True)
hgoal = None
connected = self._client.wait_for_server(rospy.Duration(1.0))
if connected:
while self._executing:
fgoal = FaceDetectorGoal()
self._pending_face_goal = True
self._client.send_goal(fgoal)
self._client.wait_for_result()
self._pending_face_goal = False
f = self._client.get_result()
if len(f.face_positions) > 0:
closest = -1
closest_dist = 1000
for i in range(len(f.face_positions)):
dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y\
+ f.face_positions[i].pos.z*f.face_positions[i].pos.z
if dist < closest_dist:
closest = i
closest_dist = dist
if closest > -1:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = f.face_positions[
closest].header.frame_id
hgoal.target.point.x = f.face_positions[closest].pos.x
hgoal.target.point.y = f.face_positions[closest].pos.y
hgoal.target.point.z = f.face_positions[closest].pos.z
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(
rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
# self._head_client.wait_for_result()
# self._pending_head_goal = False
else:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = "base_footprint"
hgoal.target.point.x = 2.0
hgoal.target.point.y = -2.0
hgoal.target.point.z = 1.2
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(
rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
self._head_client.wait_for_result()
self._pending_head_goal = False
if self._executing:
time.sleep(1.0)
self._finished_finding_face()
def _preempt(self, event):
self._executing = False
if self._pending_face_goal:
self._client.cancel_goal()
if self._pending_head_goal:
self._head_client.cancel_goal()
self._execute_finished()
def _finished_finding_face(self):
if self._executing:
self._executing = False
self._timer.shutdown()
self._execute_finished()
class DockActionServer(ActionServer):
def __init__(self, name):
ActionServer.__init__(self, name, DockAction, self.__goal_callback,
self.__cancel_callback, False)
self.__docked = False
self.__dock_speed = rospy.get_param('~dock/dock_speed', 0.05)
self.__dock_distance = rospy.get_param('~dock/dock_distance', 1.0)
self.__map_frame = rospy.get_param('~map_frame', 'map')
self.__odom_frame = rospy.get_param('~odom_frame', 'odom')
self.__base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.__dock_ready_pose = Pose()
self.__dock_ready_pose.position.x = rospy.get_param('~dock/pose_x')
self.__dock_ready_pose.position.y = rospy.get_param('~dock/pose_y')
self.__dock_ready_pose.position.z = rospy.get_param('~dock/pose_z')
self.__dock_ready_pose.orientation.x = rospy.get_param(
'~dock/orientation_x')
self.__dock_ready_pose.orientation.y = rospy.get_param(
'~dock/orientation_y')
self.__dock_ready_pose.orientation.z = rospy.get_param(
'~dock/orientation_z')
self.__dock_ready_pose.orientation.w = rospy.get_param(
'~dock/orientation_w')
self.__dock_ready_pose_2 = PoseStamped()
rospy.loginfo('param: dock_spped, %s, dock_distance %s' %
(self.__dock_speed, self.__dock_distance))
rospy.loginfo('param: map_frame %s, odom_frame %s, base_frame %s' %
(self.__map_frame, self.__odom_frame, self.__base_frame))
rospy.loginfo('dock_pose:')
rospy.loginfo(self.__dock_ready_pose)
self.__movebase_client = SimpleActionClient('move_base',
MoveBaseAction)
rospy.loginfo('wait for movebase server...')
self.__movebase_client.wait_for_server()
rospy.loginfo('movebase server connected')
self.__cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.Subscriber('dock_pose', PoseStamped, self.__dock_pose_callback)
self.__tf_listener = tf.TransformListener()
self.__docked = False
# self.__saved_goal = MoveBaseGoal()
self.__no_goal = True
self.__current_goal_handle = ServerGoalHandle()
self.__exec_condition = threading.Condition()
self.__exec_thread = threading.Thread(None, self.__exec_loop)
self.__exec_thread.start()
rospy.loginfo('Creating ActionServer [%s]\n', name)
def __del__(self):
self.__movebase_client.cancel_all_goals()
def __dock_pose_callback(self, data):
ps = PoseStamped()
# ps.header.stamp = rospy.Time.now()
ps.header.frame_id = 'dock'
ps.pose.position.x = -self.__dock_distance
try:
self.__dock_ready_pose_2 = self.__tf_listener.transformPose(
'map', ps)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.__dock_ready_pose_2.pose.position.z = -1.0
rospy.logwarn('tf error, %s' % e)
self.__dock_ready_pose_2.pose.position.z = 0.0
# rospy.loginfo('get dock pose')
def __goal_callback(self, gh):
rospy.loginfo('get new goal')
if not self.__no_goal:
gh.set_rejected(None, 'robot is busy, rejected')
rospy.logwarn('robot is busy, rejected')
return
self.__exec_condition.acquire()
self.__current_goal_handle = gh
self.__no_goal = False
self.__exec_condition.notify()
self.__exec_condition.release()
def __set_charge_relay(self, state):
pass
def __cancel_callback(self, gh):
self.__movebase_client.cancel_goal()
rospy.logwarn('cancel callback')
def __get_delta(self, pose, target):
if pose < 0:
pose = math.pi * 2 + pose
if target < 0:
target = math.pi * 2 + target
delta_a = target - pose
delta_b = math.pi * 2 - math.fabs(delta_a)
if delta_a > 0:
delta_b = delta_b * -1.0
if math.fabs(delta_a) < math.fabs(delta_b):
return delta_a
else:
return delta_b
def __rotate(self, delta):
try:
pose, quaternion = self.__tf_listener.lookupTransform(
'odom', self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
(roll, pitch, yaw) = euler_from_quaternion(quaternion)
target_yaw = yaw + delta
if target_yaw > math.pi:
target_yaw = target_yaw - (2.0 * math.pi)
elif target_yaw < -math.pi:
target_yaw = target_yaw + 2.0 * math.pi
rospy.loginfo('rotate %f to %f', delta, target_yaw)
cmd = Twist()
time = rospy.Time.now() + rospy.Duration(15)
while rospy.Time.now() < time and not rospy.is_shutdown(
) and math.fabs(self.__get_delta(yaw, target_yaw)) > 0.03:
try:
pose, quaternion = self.__tf_listener.lookupTransform(
'odom', self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
(roll, pitch, yaw) = euler_from_quaternion(quaternion)
a = self.__get_delta(yaw, target_yaw) * 0.4
if a > 0 and a < 0.08:
cmd.angular.z = 0.08
elif a < 0 and a > -0.08:
cmd.angular.z = -0.08
else:
cmd.angular.z = a
self.__cmd_pub.publish(cmd)
rospy.loginfo('rotate %f : %f, delta %f, speed %f, %f', target_yaw,
yaw, self.__get_delta(yaw,
target_yaw), a, cmd.angular.z)
return True
def __head_align(self):
cmd = Twist()
time = rospy.Time.now() + rospy.Duration(10)
while rospy.Time.now() < time:
try:
dock_pose, dock_quaternion = self.__tf_listener.lookupTransform(
self.__base_frame, 'dock', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
# when dock's x is close to zero, it means dock is just in the front of robot(base_footprint)
if dock_pose[1] < -0.002:
cmd.angular.z = -0.08
elif dock_pose[1] > 0.002:
cmd.angular.z = 0.08
else:
cmd.angular.z = 0.00
self.__cmd_pub.publish(cmd)
break
rospy.loginfo('algin %f, speed %f', dock_pose[1], cmd.angular.z)
self.__cmd_pub.publish(cmd)
self.__cmd_pub.publish(cmd)
rospy.Rate(0.5).sleep()
return True
def __moveto_dock(self):
cmd = Twist()
cmd.linear.x = -self.__dock_speed
ca_feedback = DockFeedback()
try:
last_pose, last_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
delta_distance = 0
while delta_distance < self.__dock_distance - 0.235 and not rospy.is_shutdown(
):
self.__cmd_pub.publish(cmd)
try:
current_pose, current_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
delta_distance = math.sqrt(
math.pow(current_pose[0] - last_pose[0], 2) +
math.pow(current_pose[1] - last_pose[1], 2) +
math.pow(current_pose[2] - last_pose[2], 2))
ca_feedback.dock_feedback = 'Moving to Dock, %fm left' % (
self.__dock_distance - delta_distance)
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('Moving to Dock, %fm left' %
(self.__dock_distance - delta_distance))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error aa')
rospy.Rate(20).sleep()
ca_feedback.dock_feedback = 'Stop on Dock'
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('stop robot')
# stop robot
cmd.linear.x = 0
self.__cmd_pub.publish(cmd)
# set charge relay on
self.__set_charge_relay(True)
return True
def __moveto_dock_ready(self):
# step 1
mb_goal = MoveBaseGoal()
mb_goal.target_pose.header.stamp = rospy.Time.now()
mb_goal.target_pose.header.frame_id = self.__map_frame
mb_goal.target_pose.header.seq = 1
mb_goal.target_pose.pose = self.__dock_ready_pose
self.__movebase_client.send_goal(mb_goal)
self.__movebase_client.wait_for_result()
rospy.loginfo('arrived dock_ready_pose')
rospy.Rate(2).sleep()
mb_goal = MoveBaseGoal()
mb_goal.target_pose.header.seq = 2
mb_goal.target_pose.header.stamp = rospy.Time.now()
mb_goal.target_pose.header.frame_id = self.__map_frame
if self.__dock_ready_pose_2.pose.position.z == -1.0:
rospy.logwarn('dock_ready_pose_2 failed')
return False
else:
rospy.loginfo('get dock ready pose 2 ()()')
t = self.__dock_ready_pose_2.pose
# t.position.z == 0.0
# t.position.x = -self.__dock_distance
mb_goal.target_pose.pose = t
rospy.loginfo('move to dock_ready_pose_2')
self.__movebase_client.cancel_all_goals()
self.__movebase_client.send_goal(mb_goal)
rospy.loginfo(self.__movebase_client.wait_for_result())
rospy.loginfo('arrived dock_ready_pose_2')
return True
def __dock(self):
if self.__moveto_dock_ready():
if self.__head_align():
if self.__rotate(math.pi):
if self.__moveto_dock():
self.__docked = True
return True
else:
rospy.logwarn("unable to move to dock")
else:
rospy.logwarn("unable to rotate 180")
else:
rospy.logwarn("unable to align head")
else:
rospy.logwarn("unable to move to dock ready")
self.__docked = False
return False
def __undock(self):
cmd = Twist()
cmd.linear.x = self.__dock_speed
ca_feedback = DockFeedback()
try:
last_pose, last_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
delta_distance = 0
while delta_distance < self.__dock_distance - 0.275 and not rospy.is_shutdown(
):
self.__cmd_pub.publish(cmd)
try:
current_pose, current_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
delta_distance = math.sqrt(
math.pow(current_pose[0] - last_pose[0], 2) +
math.pow(current_pose[1] - last_pose[1], 2) +
math.pow(current_pose[2] - last_pose[2], 2))
ca_feedback.dock_feedback = 'Undock, %fm left' % (
self.__dock_distance - delta_distance)
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('Undock, %fm left' %
(self.__dock_distance - delta_distance))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error aa')
rospy.Rate(20).sleep()
ca_feedback.dock_feedback = 'Stop on DockReady'
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('stop robot')
# stop robot
cmd.linear.x = 0.0
self.__cmd_pub.publish(cmd)
# set charge relay off
self.__set_charge_relay(False)
self.__docked = False
self.__current_goal_handle.set_succeeded(None, 'Undocked')
rospy.loginfo('UnDocked')
def __exec_loop(self):
rospy.loginfo('auto dock thread started')
while not rospy.is_shutdown():
with self.__exec_condition:
self.__exec_condition.wait(3)
if self.__no_goal:
continue
rospy.loginfo('processing new goal')
goal = self.__current_goal_handle.get_goal()
if goal.dock == True:
if self.__docked == True:
rospy.logwarn('rejected, robot has already docked')
self.__current_goal_handle.set_rejected(
None, 'already docked')
else:
rospy.loginfo('Docking')
self.__current_goal_handle.set_accepted('Docking')
self.__dock()
if self.__docked:
self.__current_goal_handle.set_succeeded(
None, 'Docked')
rospy.loginfo('Docked')
else:
self.__current_goal_handle.set_aborted(
None, 'Dock failed')
rospy.loginfo('Dock failed')
elif goal.dock == False:
if self.__docked == False:
rospy.logwarn('cancel_all_goals')
self.__movebase_client.cancel_all_goals()
rospy.logwarn('rejected, robot is not on charging')
self.__current_goal_handle.set_rejected(
None, 'robot is not on charging')
else:
rospy.loginfo('Start undock')
self.__current_goal_handle.set_accepted('Start undock')
self.__undock()
else:
rospy.logwarn(
'unknown dock data type, should be true or false')
# self.__current_goal_handle.set_succeeded(None, 'Docked')
rospy.loginfo('new goal finish')
self.__no_goal = True
rospy.loginfo('auto dock thread stop')
class TestMoveActionCancelDrop(unittest.TestCase):
def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def test_cancel_drop_plan_execution(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED
or result.error_code.val == 0)
def test_cancel_drop_plan_only(self):
# set the plan only flag
self._goal.planning_options.plan_only = True
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED
or result.error_code.val == 0)
def test_cancel_resend(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# send the goal again
self._move_client.send_goal(self._goal)
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
class SimpleExerciser(unittest.TestCase):
def setUp(self):
self.default_wait = rospy.Duration(60.0)
self.client = SimpleActionClient('test_request_action',
TestRequestAction)
self.assert_(self.client.wait_for_server(self.default_wait))
def test_just_succeed(self):
goal = TestRequestGoal(
terminate_status=TestRequestGoal.TERMINATE_SUCCESS, the_result=42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_abort(self):
goal = TestRequestGoal(
terminate_status=TestRequestGoal.TERMINATE_ABORTED, the_result=42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_preempt(self):
goal = TestRequestGoal(
terminate_status=TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate=rospy.Duration(100000),
the_result=42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
# Should print out errors about not setting a terminal status in the action server.
def test_drop(self):
goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_DROP,
the_result=42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
# Should print out errors about throwing an exception
def test_exception(self):
goal = TestRequestGoal(
terminate_status=TestRequestGoal.TERMINATE_EXCEPTION,
the_result=42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
def test_ignore_cancel_and_succeed(self):
goal = TestRequestGoal(
terminate_status=TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate=rospy.Duration(2.0),
ignore_cancel=True,
the_result=42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_lose(self):
goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_LOSE,
the_result=42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.LOST, self.client.get_state())
class StateMachine(object):
def __init__(self):
self.node_name = "Student SM"
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
self.pick_server_name = rospy.get_param(rospy.get_name() + '/pick_srv')
self.place_server_name = rospy.get_param(rospy.get_name() +
'/place_srv')
self.cube_pose = rospy.get_param(rospy.get_name() +
'/cube_pose').split(',')
#self.cube_pose_topic = '/manipulation_client/marker_pose_topic'
self.cube_pose_topic = rospy.get_param(rospy.get_name() +
'/aruco_pose_topic')
#the picking and placing poses of the robot
#self.pick_pose_robot = rospy.get_param(rospy.get_name()+ '/pick_pose')
#self.place_pose_robot = rospy.get_param(rospy.get_name()+ '/place_pose')
# Subscribe to topics
# here we need to add attributes for the different subscribers we are going to use
self.place_marker_pose = rospy.get_param(rospy.get_name() +
'/place_pose_topic')
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
rospy.wait_for_service(self.pick_server_name, timeout=30)
rospy.wait_for_service(self.place_server_name, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
self.cube_pose_publisher = rospy.Publisher(self.cube_pose_topic,
PoseStamped,
queue_size=10)
self.place_pose_publisher = rospy.Publisher(self.place_marker_pose,
PoseStamped,
queue_size=10)
self.rate = rospy.Rate(10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
# Init state machine
self.state = 0
rospy.sleep(3)
self.check_states()
def check_states(self):
print("running check_states in sm_students.py")
while not rospy.is_shutdown() and self.state != 4:
#state 0 tuck the arm
if self.state == 0:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
success_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(100.0))
if success_tucking:
rospy.loginfo("%s: Arm tuck: ",
self.play_motion_ac.get_result())
self.state = 1
else:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = 5
rospy.sleep(1)
#state 1: pick the cube
if self.state == 1:
try:
msg = PoseStamped()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
msg.header.seq = 1
msg.pose.position.x = float(self.cube_pose[0])
msg.pose.position.y = float(self.cube_pose[1])
msg.pose.position.z = float(self.cube_pose[2])
msg.pose.orientation.x = float(self.cube_pose[3])
msg.pose.orientation.y = float(self.cube_pose[4])
msg.pose.orientation.z = float(self.cube_pose[5])
msg.pose.orientation.w = float(self.cube_pose[6])
self.cube_pose_publisher.publish(msg)
self.rate.sleep()
rospy.loginfo("%s: Picking cube...", self.node_name)
pick_cube_server = rospy.ServiceProxy(
self.pick_server_name,
SetBool) #not sure if its supposed to be SetBool here
pick_cube_server.wait_for_service()
pick_up_req = pick_cube_server()
if pick_up_req.success == True:
self.state = 2
rospy.loginfo("%s: Picking cube succeded!",
self.node_name)
else:
rospy.loginfo("%s: Picking cube failed!",
self.node_name)
self.state = 999
rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
if self.state == 2:
#move the robot towards the other table
move_msg = Twist()
move_msg.angular.z = -1
converged = False
cnt = 0
rospy.loginfo("%s: Moving towards table", self.node_name)
while not rospy.is_shutdown() and cnt < 31:
self.cmd_vel_pub.publish(move_msg)
self.rate.sleep()
cnt = cnt + 1
move_msg.linear.x = 1
move_msg.angular.z = 0
cnt = 0
rospy.sleep(2)
while not rospy.is_shutdown(
) and cnt < 10: #i changed this back to 10 cause the robot kept touching the table
self.cmd_vel_pub.publish(move_msg)
self.rate.sleep()
cnt = cnt + 1
rospy.loginfo("%s: Navigation towards table completed",
self.node_name)
self.state = 3
rospy.sleep(1)
# state 3: place the cube
if self.state == 3:
try:
rospy.loginfo("%s: Placing cube...", self.node_name)
place_cube_server = rospy.ServiceProxy(
self.place_server_name,
SetBool) #not sure if its supposed to be SetBool here
place_cube_server.wait_for_service()
place_req = place_cube_server()
if place_req.success == True:
self.state = 4
rospy.loginfo("%s: Placing the cube succeded!",
self.node_name)
else:
rospy.logerr("%s: Placing the cube failed!",
self.node_name)
self.state = 3
rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to place server failed: %s" % e
# Error handling
if self.state == 999:
rospy.logerr(
"%s: State machine failed. Check your code and try again!",
self.node_name)
return
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
def __init__(self, context):
self.prompt_width = 170
self.input_width = 250
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._sound_client = SoundClient()
#find relative path for files to load
self.local_dir = os.path.dirname(__file__)
self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/')
if not os.path.isdir(self.dir):
os.makedirs(self.dir)
#need to add any additional subfolders as needed
if not os.path.isdir(self.dir + 'animations/'):
os.makedirs(self.dir + 'animations/')
# Creates a subscriber to the ROS topic, having msg type SoundRequest
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Code used for saving/ loading arm poses for the robot
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self.all_joint_names = []
self.all_joint_poses = []
# Hash tables storing the name of the pose and the
# associated positions for that pose, initially empty
self.saved_l_poses = {}
self.saved_r_poses = {}
# Hash tables for storing name of animations and the associated pose list
self.saved_animations = {}
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
#parsing for animations
dir = os.path.dirname(__file__)
qWarning(dir)
filename = os.path.join(self.dir, 'animations/')
ani_path = filename
ani_listing = os.listdir(ani_path)
for infile in ani_listing:
pose_left = []
pose_right = []
left_gripper_states = []
right_gripper_states = []
line_num = 1
for line in fileinput.input(ani_path + infile):
if (line_num % 2 == 1):
pose = [float(x) for x in line.split()]
pose_left.append(pose[:len(pose)/2])
pose_right.append(pose[len(pose)/2:])
else:
states = line.split()
left_gripper_states.append(states[0])
right_gripper_states.append(states[1])
line_num += 1
self.saved_animations[os.path.splitext(infile)[0]] = Quad(pose_left, pose_right, left_gripper_states, right_gripper_states)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
# Navigation functionality initialization
self.roomNav = RoomNavigator()
self._tf_listener = TransformListener()
self.animPlay = AnimationPlayer(None, None, None, None)
# Detection and pickup functionality
self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav, self.animPlay)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# Create a large vertical box that is top aligned
large_box = QtGui.QVBoxLayout()
large_box.setAlignment(QtCore.Qt.AlignTop)
large_box.setMargin(0)
large_box.addItem(QtGui.QSpacerItem(10,0))
# Buttons for controlling the head of the robot
head_box = QtGui.QHBoxLayout()
head_box.addItem(QtGui.QSpacerItem(230,0))
head_box.addWidget(self.create_pressed_button('Head Up'))
head_box.addStretch(1)
large_box.addLayout(head_box)
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(80,0))
button_box.addWidget(self.create_pressed_button('Head Turn Left'))
button_box.addWidget(self.create_pressed_button('Head Down'))
button_box.addWidget(self.create_pressed_button('Head Turn Right'))
button_box.addStretch(1)
button_box.setMargin(0)
button_box.setSpacing(0)
large_box.addLayout(button_box)
# Shows what the robot says
speech_box = QtGui.QHBoxLayout()
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
self.speech_label.setPalette(palette) #
speech_box.addItem(QtGui.QSpacerItem(100,0))
#speech_box.addWidget(self.speech_label) #
large_box.addLayout(speech_box)
# Speak button
speak_button_box = QtGui.QHBoxLayout();
speech_prompt = QtGui.QLabel('Enter Speech Text:')
speech_prompt.setFixedWidth(self.prompt_width)
speak_button_box.addWidget(speech_prompt)
robot_says = QtGui.QLineEdit(self._widget)
robot_says.setFixedWidth(self.input_width)
robot_says.textChanged[str].connect(self.onChanged) #
speak_button_box.addWidget(robot_says)
speak_button_box.addWidget(self.create_button('Speak'))
speak_button_box.addStretch(1)
large_box.addLayout(speak_button_box)
large_box.addItem(QtGui.QSpacerItem(0,50))
# Buttons for arm poses
pose_button_box1 = QtGui.QHBoxLayout()
pose_button_box1.addItem(QtGui.QSpacerItem(150,0))
pose_button_box1.addWidget(self.create_button('Relax Left Arm'))
pose_button_box1.addWidget(self.create_button('Relax Right Arm'))
pose_button_box1.addStretch(1)
large_box.addLayout(pose_button_box1)
# Buttons for grippers
gripper_button_box = QtGui.QHBoxLayout()
gripper_button_box.addItem(QtGui.QSpacerItem(150,0))
gripper_button_box.addWidget(self.create_button('Open Left Gripper'))
gripper_button_box.addWidget(self.create_button('Open Right Gripper'))
gripper_button_box.addStretch(1)
large_box.addLayout(gripper_button_box)
large_box.addItem(QtGui.QSpacerItem(0,25))
# Buttons for animation
animation_box = QtGui.QHBoxLayout()
play_anim_label = QtGui.QLabel('Select Animation:')
play_anim_label.setFixedWidth(self.prompt_width)
animation_box.addWidget(play_anim_label)
self.saved_animations_list = QtGui.QComboBox(self._widget)
animation_box.addWidget(self.saved_animations_list)
pose_time_label = QtGui.QLabel('Duration(sec):')
pose_time_label.setFixedWidth(100)
animation_box.addWidget(pose_time_label)
self.pose_time = QtGui.QLineEdit(self._widget)
self.pose_time.setFixedWidth(50)
self.pose_time.setText('2.0')
animation_box.addWidget(self.pose_time)
animation_box.addWidget(self.create_button('Play Animation'))
animation_box.addStretch(1)
large_box.addLayout(animation_box)
animation_box2 = QtGui.QHBoxLayout()
animation_name_label = QtGui.QLabel('Enter Animation Name:')
animation_name_label.setFixedWidth(self.prompt_width)
animation_box2.addWidget(animation_name_label)
self.animation_name = QtGui.QLineEdit(self._widget)
self.animation_name.setFixedWidth(self.input_width)
animation_box2.addWidget(self.animation_name)
animation_box2.addWidget(self.create_button('Save Animation'))
animation_box2.addStretch(1)
large_box.addLayout(animation_box2)
animation_box3 = QtGui.QHBoxLayout()
pose_name_label = QtGui.QLabel('Enter Pose Name:')
pose_name_label.setFixedWidth(self.prompt_width)
animation_box3.addWidget(pose_name_label)
self.pose_name_temp = QtGui.QLineEdit(self._widget)
self.pose_name_temp.setFixedWidth(self.input_width)
animation_box3.addWidget(self.pose_name_temp)
animation_box3.addWidget(self.create_button('Add Current Pose'))
animation_box3.addStretch(1)
large_box.addLayout(animation_box3)
# Playing around with UI stuff
play_box = QtGui.QHBoxLayout()
pose_sequence_label = QtGui.QLabel('Current Pose Sequence:')
pose_sequence_label.setFixedWidth(self.prompt_width)
pose_sequence_label.setAlignment(QtCore.Qt.AlignTop)
self.list_widget = QListWidget()
self.list_widget.setDragDropMode(QAbstractItemView.InternalMove)
self.list_widget.setMaximumSize(self.input_width, 200)
play_box.addWidget(pose_sequence_label)
play_box.addWidget(self.list_widget)
play_box.addStretch(1)
large_box.addLayout(play_box)
large_box.addItem(QtGui.QSpacerItem(0,50))
# Buttons for first row of base controls
first_base_button_box = QtGui.QHBoxLayout()
first_base_button_box.addItem(QtGui.QSpacerItem(70,0))
first_base_button_box.addWidget(self.create_pressed_button('Rotate Left'))
first_base_button_box.addWidget(self.create_pressed_button('^'))
first_base_button_box.addWidget(self.create_pressed_button('Rotate Right'))
first_base_button_box.addStretch(1)
large_box.addLayout(first_base_button_box)
# Buttons for second row of base controls
second_base_button_box = QtGui.QHBoxLayout()
second_base_button_box.addItem(QtGui.QSpacerItem(70,0))
second_base_button_box.addWidget(self.create_pressed_button('<'))
second_base_button_box.addWidget(self.create_pressed_button('v'))
second_base_button_box.addWidget(self.create_pressed_button('>'))
second_base_button_box.addWidget(self.create_button('Move to Bin'))
second_base_button_box.addWidget(self.create_button('Object Detect'))
second_base_button_box.addWidget(self.create_button('Clean Room'))
second_base_button_box.addStretch(1)
large_box.addLayout(second_base_button_box)
# Animation related items to store intermediate pose co-ordinates to save
self.animation_map = {}
self.create_state = False
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
# Look straight and down when launched
self.head_x = 1.0
self.head_y = 0.0
self.head_z = 0.5
self.head_action(self.head_x, self.head_y, self.head_z)
# Set grippers to closed on initialization
self.left_gripper_state = ''
self.right_gripper_state = ''
self.gripper_action('l', 0.0)
self.gripper_action('r', 0.0)
# Lab 6
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Saved states for poses
saved_pose_box = QtGui.QHBoxLayout()
self.saved_left_poses = QtGui.QLabel('')
self.saved_right_poses = QtGui.QLabel('')
saved_pose_box.addWidget(self.saved_left_poses)
saved_pose_box.addWidget(self.saved_right_poses)
large_box.addLayout(saved_pose_box)
# Preload the map of animations
self.saved_animations_list.addItems(self.saved_animations.keys())
# Move the torso all the way down
# self.torso_down(True)
# Autonomous navigation stuff
'''
self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)),
Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)),
Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)),
Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)),
Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)),
Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)),
Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)),
Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))]
'''
self.locations = [Pose(Point(2.04748392105, 2.04748010635, 0.000), Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)),
Pose(Point(2.34193611145, 1.43208932877, 0), Quaternion(0, 0, -0.841674385779, 0.539985396398)),
Pose(Point(3.43202018738, -0.258297920227, 0.000), Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)),
Pose(Point(0.931655406952, -1.96435832977, 0.000), Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)),
Pose(Point(-0.369112968445, 0.0330476760864, 0.000), Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))]
self.index = 1;
rospy.loginfo("Completed GUI initialization")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text() )
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' % (old_arm_state, arm_side))
elif('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(self.get_joint_state('l'), self.get_joint_state('r'), self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif('Move to Bin' == button_name):
self.move_to_bin_action()
elif('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while(self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if(map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point = None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose, map_point.header.frame_id, '/base_link')
rospy.loginfo("Object is " + str (map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False;
if(map_point.pose.position.x < 0.7):
move_back_first = True;
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose, '/base_link', '/map')
if(move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if(success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations['ready_pickup'].left
self.animPlay.right_poses = self.saved_animations['ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations['ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range (0,3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined', self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0],trans[0][1],trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait = False):
self.torso_client = SimpleActionClient('/torso_controller/position_joint_action', SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Torso movement goal failed with error code: " + str(state))
def head_action(self, x, y, z, wait = False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations['left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations['l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations['l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(re.sub(',' , '', str(self.animation_map[pose_name].left).strip('[]') +
' ' + str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(str(self.animation_map[pose_name].left_gripper) + ' ' + str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(l_animation_queue, r_animation_queue, l_gripper_queue, r_gripper_queue)
self.saved_animations_list.addItem(animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz("%sing %s Gripper" % (grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText('%s %s Gripper' % (grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
class StateMachine(object):
def aruco_pose_cb(self, aruco_pose_msg):
self.aruco_pose = aruco_pose_msg
self.aruco_pose_rcv = True
def gripper_cb(self, joint_state_msg):
self.left_gripper = joint_state_msg.position[7]
self.right_gripper = joint_state_msg.position[8]
def __init__(self):
self.node_name = "Student SM"
self.aruco_pose = None
self.aruco_pose_rcv = False
self.left_gripper = None
self.right_gripper = None
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
self.pk_srv = rospy.get_param(rospy.get_name() + '/pick_srv')
self.plc_srv = rospy.get_param(rospy.get_name() + '/place_srv')
self.dtct_top = rospy.get_param(rospy.get_name() + '/aruco_pose_topic')
self.lclz_srv = rospy.get_param(rospy.get_name() + '/global_loc_srv')
self.clrcstmp_srv = rospy.get_param(rospy.get_name() +
'/clear_costmaps_srv')
self.pkpose_top = rospy.get_param(rospy.get_name() +
'/pick_pose_topic')
self.plcpose_top = rospy.get_param(rospy.get_name() +
'/place_pose_topic')
# Subscribe to topics
self.dtct_sub = rospy.Subscriber(self.dtct_top, PoseStamped,
self.aruco_pose_cb)
self.gripper_sub = rospy.Subscriber("/joint_states", JointState,
self.gripper_cb)
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
rospy.wait_for_service(self.pk_srv, timeout=30)
rospy.wait_for_service(self.plc_srv, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
rospy.loginfo("%s: Waiting for move base goal server...",
self.node_name)
self.move_base_ac = SimpleActionClient("/move_base", MoveBaseAction)
if not self.move_base_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /move_base action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to move_base action server",
self.node_name)
# Init state machine
self.state = 0
rospy.sleep(3)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state != 50:
# State 0: Tuck arm
if self.state == 0:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
fail_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(10.0))
if fail_tucking:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
previous_state = 0
self.state = 0
else:
rospy.loginfo("%s: Arm tucked.", self.node_name)
previous_state = 0
self.state = 1
rospy.sleep(1)
# State 1: Localize robot
if self.state == 1:
move_msg = Twist()
move_msg.angular.z = -1
rospy.loginfo("%s: Localizing Rob position...", self.node_name)
localize_srv = rospy.ServiceProxy(self.lclz_srv, Empty)
localize_req = localize_srv()
rate = rospy.Rate(10)
converged = False
cnt = 0
rospy.loginfo("%s: Doing a Barrel Roll...", self.node_name)
while not rospy.is_shutdown() and cnt < 60:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
clear_costmap_srv = rospy.ServiceProxy(self.clrcstmp_srv,
Empty)
clear_costamp_req = clear_costmap_srv()
previous_state = 1
self.state = 2
rospy.sleep(1)
# State 2: Navigate to the cube
if self.state == 2:
rospy.loginfo("%s: Navigating to cube...", self.node_name)
pose = rospy.wait_for_message(self.pkpose_top, PoseStamped, 5)
goal = MoveBaseGoal()
goal.target_pose = pose
self.move_base_ac.send_goal(goal)
success_navigation = self.move_base_ac.wait_for_result(
rospy.Duration(30.0))
if not success_navigation:
self.move_base_ac.cancel_goal()
rospy.logerr(
"%s: Failed to navigate to cube, reset simulation",
self.node_name)
previous_state = 2
self.state = 3
else:
rospy.loginfo("%s: Cube reached!", self.node_name)
previous_state = 2
self.state = 3
rospy.sleep(1)
# State 3: Lower robot head service
if self.state == 3:
try:
rospy.loginfo("%s: Lowering robot head...", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
rospy.loginfo("%s: Move head down succeded!",
self.node_name)
previous_state = 3
self.state = 7
else:
rospy.loginfo("%s: Move head down failed!",
self.node_name)
previous_state = 3
self.state = 404
rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
# State 4: Pick up the cube service
if self.state == 4:
try:
rospy.loginfo("%s: Picking the cube...", self.node_name)
pick_up_srv = rospy.ServiceProxy(self.pk_srv, SetBool)
pick_up_req = pick_up_srv()
if pick_up_req.success == True:
previous_state = 4
self.state = 7
else:
rospy.loginfo("%s: Pick up failed!", self.node_name)
previous_state = 4
self.state = 404
rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to pick_up server failed: %s" % e
# State 5: Navigate to table A
if self.state == 5:
rospy.loginfo("%s: Navigating to table...", self.node_name)
pose = rospy.wait_for_message(self.plcpose_top, PoseStamped, 5)
goal = MoveBaseGoal()
goal.target_pose = pose
self.move_base_ac.send_goal(goal)
success_navigation = self.move_base_ac.wait_for_result(
rospy.Duration(120.0))
if not success_navigation:
self.move_base_ac.cancel_goal()
rospy.logerr(
"%s: Failed to navigate to table, reset simulation",
self.node_name)
previous_state = 5
self.state = 404
else:
rospy.loginfo("%s: Table reached!", self.node_name)
previous_state = 5
self.state = 6
rospy.sleep(1)
# State 6: Place the cube service
if self.state == 6:
try:
rospy.loginfo("%s: Placing the cube...", self.node_name)
place_srv = rospy.ServiceProxy(self.plc_srv, SetBool)
place_req = place_srv()
if place_req.success == True:
previous_state = 6
self.state = 7
rospy.loginfo("%s: Placing succeded!", self.node_name)
else:
rospy.loginfo("%s: Placing failed!", self.node_name)
previous_state = 6
self.state = 404
rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to pick_up server failed: %s" % e
class StateMachine(object):
def callback_pick_pos(self, msg):
self.next_goal.target_pose = msg
def callback_place_pos(self, msg):
self.place_goal.target_pose = msg
def pos_check_aruco_there(self, msg):
self.time_data = msg.header.stamp.secs
self.z_coordinate_cube = msg.pose.position.z
def check_it_get_table(self, msg):
self.x_coord_robot = msg.pose.pose.position.x
self.y_coord_robot = msg.pose.pose.position.y
def __init__(self):
self.node_name = "Student SM"
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
self.pick_srv_nm = rospy.get_param(rospy.get_name() + '/pick_srv')
# Added Hector
self.place_srv_nm = rospy.get_param(rospy.get_name() + '/place_srv')
self.pick_top = rospy.get_param(rospy.get_name() + '/pick_pose_topic')
self.place_top = rospy.get_param(rospy.get_name() +
'/place_pose_topic')
self.aruco_pose_top = rospy.get_param(rospy.get_name() +
'/aruco_pose_topic')
self.cube_pose = rospy.get_param(rospy.get_name() + '/cube_pose')
self.localize_myself = rospy.get_param(rospy.get_name() +
'/global_loc_srv')
self.clear_cost_map = rospy.get_param(rospy.get_name() +
'/clear_costmaps_srv')
rospy.loginfo("%s: ...A...", self.node_name)
# Subscribe to topics
self.next_goal = MoveBaseGoal()
self.next_goal.target_pose.header.frame_id = "map"
self.pick_pose_sub = rospy.Subscriber(self.pick_top, PoseStamped,
self.callback_pick_pos)
self.place_goal = MoveBaseGoal()
self.place_goal.target_pose.header.frame_id = "map"
self.place_pose_sub = rospy.Subscriber(self.place_top, PoseStamped,
self.callback_place_pos)
self.check_coord = rospy.Subscriber(self.aruco_pose_top, PoseStamped,
self.pos_check_aruco_there)
# ADDED AT LAST movement
self.check_coord = rospy.Subscriber("/amcl_pose",
PoseWithCovarianceStamped,
self.check_it_get_table)
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
# ADDED HECTORstd_srvs
rospy.wait_for_service(self.pick_srv_nm, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
#self.cmd_vel_pub = rospy.Publisher("move_base/goal", Twist, queue_size=10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
self.move_to_dir = SimpleActionClient("/move_base", MoveBaseAction)
if not self.move_to_dir.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /move_base action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to move_base action server",
self.node_name)
# Init state machine
self.state = 0
#rospy.sleep(1)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state != 6:
# State 0: Tuck arm
if self.state == 0:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
fail_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(10.0))
if fail_tucking:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = 6
else:
rospy.loginfo("%s: Arm tucked.", self.node_name)
self.state = 9
#rospy.sleep(1)
# State 9: Localize myself
if self.state == 9:
rospy.loginfo("%s: STATE 0", self.node_name)
localize_myself_var = rospy.ServiceProxy(
self.localize_myself, Empty)
localize_req = localize_myself_var()
rospy.loginfo("%s: Localization initialized", self.node_name)
self.state = 10
#rospy.sleep(1)
# State 10: Turn around in order to localize in the right direction
if self.state == 10:
rate = rospy.Rate(10)
movement = Twist()
movement.angular.z = 1
cnt = 0
while cnt < 20:
self.cmd_vel_pub.publish(movement)
rate.sleep()
cnt = cnt + 1
self.state = 11
rospy.loginfo(
"%s: After turning Im supposed to know where am I",
self.node_name)
#rospy.sleep(1)
# State 11: Clear costmaps
if self.state == 11:
clear_cost_m = rospy.ServiceProxy(self.clear_cost_map, Empty)
clear_cost_m()
self.state = 12
rospy.loginfo("%s: Costmaps cleared", self.node_name)
#rospy.sleep(1)
# State 12: Send navigation goal
if self.state == 12:
self.move_to_dir.wait_for_server()
self.move_to_dir.send_goal(self.next_goal)
reached_point = self.move_to_dir.wait_for_result()
#rospy.sleep(1)
if reached_point:
self.move_to_dir.cancel_goal()
rospy.loginfo("%s: I reach the table to pick the cube",
self.node_name)
self.state = 13
else:
rospy.logerr("%s: I couldnt reach the table",
self.node_name)
self.state = 6
# State 13: Move head down
if self.state == 13:
try:
rospy.loginfo("%s: Lowering robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
self.state = 14
rospy.loginfo("%s: Move head down succeded!",
self.node_name)
else:
rospy.logerr("%s: Move head down failed!",
self.node_name)
self.state = 6
#rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
# State 14: Pick cube
if self.state == 14:
rospy.loginfo("%s: Picking the cube...", self.node_name)
pick_the_cube = rospy.ServiceProxy(self.pick_srv_nm, SetBool)
pick_cube_req = pick_the_cube(True)
if pick_cube_req.success == True:
self.state = 15
rospy.loginfo("%s: Pick cube succeded!", self.node_name)
else:
rospy.loginfo("%s: Pick cube failed!", self.node_name)
self.state = 6
#rospy.sleep(1)
# State 15: Clear costmaps
if self.state == 15:
clear_cost_m = rospy.ServiceProxy(self.clear_cost_map, Empty)
clear_cost_m()
self.state = 16
rospy.loginfo("%s: Costmaps cleared", self.node_name)
#rospy.sleep(1)
# State 16: Heads up
if self.state == 16:
try:
rospy.loginfo("%s: Upping robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("up")
if move_head_req.success == True:
self.state = 17
rospy.loginfo("%s: Move head doupwn succeded!",
self.node_name)
else:
rospy.logerr("%s: Move head up failed!",
self.node_name)
self.state = 6
#rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
# State 15: Move the robot to chair
if self.state == 17:
self.move_to_dir.wait_for_server()
self.move_to_dir.send_goal(self.place_goal)
reached_point = self.move_to_dir.wait_for_result()
rospy.sleep(1)
if reached_point:
self.move_to_dir.cancel_goal()
rospy.logerr("%s: I reach the table to place the cube",
self.node_name)
self.state = 21
else:
rospy.loginfo(
"%s: I couldnt reach the table to place the cube",
self.node_name)
self.state = 6
#rospy.sleep(1)
# ADDED AT THE END
if self.state == 21:
if (((self.x_coord_robot - 2.6009)**2) +
((self.y_coord_robot + 1.7615)**2) > 1):
rospy.logerr("%s: DESTINATION NOT REACHED", self.node_name)
self.state = 6
else:
rospy.loginfo("%s: I REACH THE TABLE", self.node_name)
self.state = 18
# State 18: Lower robot head service
if self.state == 18:
try:
rospy.loginfo("%s: Lowering robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
self.state = 19
rospy.loginfo("%s: Move head down succeded!",
self.node_name)
else:
rospy.logerr("%s: Move head down failed!",
self.node_name)
self.state = 6
#rospy.sleep(1)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
class BakerArmServer(script):
DOF = 5
DEFAULT_POSITION = [0.6, 1., -2., 1., 0.]
TRANSPORT_POSITION = [0.9, 1., -2., 1., 0.]
IS_GRIPPER_AVAILABLE = False
def __init__(self, name, status=ArmStatus.NO_TRASHCAN):
self.verbose_ = True
self.confirm_ = False
self.name_ = name
self.status_ = status
self.joint_values_ = [0.0] * self.DOF
self.mutex_ = Lock()
(self.trashcan_pose_, self.trolley_pose_) = (None, None)
self.displayParameters()
self.initClients()
self.initServices()
self.initServers()
self.initSubscribers()
self.initTopics()
def confirm(self):
if not self.confirm_:
return
raw_input("Please press enter to confirm")
def displayParameters(self):
if not self.verbose_:
return
print("========== baker_arm_server Parameters ==========")
print("todo")
@log
def Initialize(self):
self.sss.init('gripper')
self.sss.init('arm')
@log
def initClients(self):
# Warning: wait_for_server and wait_for_service are not the same function.
# * rospy.duration expected for wait_for_server
# * float expected for wait_for_service
self.plan_client_ = SimpleActionClient('/arm_planning_node/PlanTo',
PlanToAction)
self.plan_client_.wait_for_server(timeout=rospy.Duration(5.0))
self.execution_client_ = SimpleActionClient('/traj_exec',
ExecuteTrajectoryAction)
self.execution_client_.wait_for_server(timeout=rospy.Duration(5.0))
self.small_gripper_finger_client_ = rospy.ServiceProxy(
"/gripper/driver/set_object", SetObject)
# rmb-ma. keep commented
# self.small_gripper_finger_client_.wait_for_service(timeout=5.0)
self.add_collision_object_client_ = rospy.ServiceProxy(
"/ipa_planning_scene_creator/add_collision_objects",
AddCollisionObject)
self.add_collision_object_client_.wait_for_service(timeout=5.0)
self.attach_object_client_ = rospy.ServiceProxy(
"ipa_planning_scene_creator/attach_object", AddCollisionObject)
self.attach_object_client_.wait_for_service(timeout=5.0)
self.detach_object_client_ = rospy.ServiceProxy(
"ipa_planning_scene_creator/detach_object", RemoveCollisionObject)
self.detach_object_client_.wait_for_service(timeout=5.0)
@log
def initServers(self):
self.to_joints_position_server_ = SimpleActionServer(
self.name_ + '/set_joints_values', ExecuteTrajectoryAction,
self.moveToJointsPositionCallback, False)
self.to_joints_position_server_.start()
self.to_transport_position_server_ = SimpleActionServer(
self.name_ + '/transport_position', MoveToAction,
self.moveToTransportPositionCallback, False)
self.to_transport_position_server_.start()
self.to_rest_position_server_ = SimpleActionServer(
self.name_ + '/rest_position', MoveToAction,
self.moveToRestPositionCallback, False)
self.to_rest_position_server_.start()
self.catch_trashcan_server_ = SimpleActionServer(
self.name_ + '/catch_trashcan', MoveToAction,
self.catchTrashcanCallback, False)
self.catch_trashcan_server_.start()
self.leave_trashcan_server_ = SimpleActionServer(
self.name_ + '/leave_trashcan', MoveToAction,
self.leaveTrashcanCallback, False)
self.leave_trashcan_server_.start()
self.empty_trashcan_server_ = SimpleActionServer(
self.name_ + '/empty_trashcan', MoveToAction,
self.emptyTrashcanCallback, False)
self.empty_trashcan_server_.start()
@log
def initServices(self):
pass
@log
def initTopics(self):
Thread(target=self.statusTalker).start()
@log
def statusTalker(self):
publisher = rospy.Publisher(self.name_ + '/status',
Int32,
queue_size=10)
rate = rospy.Rate(5)
while not rospy.is_shutdown():
publisher.publish(self.status_.value)
rate.sleep()
@log
def initSubscribers(self):
rospy.Subscriber("/arm/joint_states", JointState,
self.jointStateCallback)
def jointStateCallback(self, msg):
'''
getting current position of the joint
@param msg containts joint position, velocity, effort, names
'''
for i in range(0, len(msg.position)):
self.joint_values_[i] = msg.position[i]
def gripperHandler(self,
finger_value=0.01,
finger_open=False,
finger_close=True):
'''
Gripper handler function use to control gripper open/close command,
move parallel part of gripper with give values (in cm)
@param finger_value is the big parallel part move realtive to zero position
@finger_open open the small finger
@finger_close close the small finger
'''
if finger_open or finger_close:
# set_object request, both the finger move together (at the same time)
request = SetObjectRequest()
response = SetObjectResponse()
request.node = 'gripper_finger1_joint'
request.object = '22A0'
if finger_open:
request.value = '5'
elif finger_close:
request.value = '10'
request.cached = False
response = self.small_gripper_finger_client_.call(request)
if response.success:
rospy.loginfo(response.message)
else:
rospy.logerr(response.message)
rospy.logerr("SetObject service call Failed!!")
# default blocking is true, wait until if finishes it tasks
self.sss.move('gripper', [[finger_value]])
return response.success
@log
def handleCheckAccessibility(self, request):
response = TriggerResponse()
response.success = True
return response
@log
def openGripper(self):
if not self.IS_GRIPPER_AVAILABLE:
return
self.gripperHandler(finger_value=0.01,
finger_open=True,
finger_close=False)
@log
def closeGripper(self):
if not self.IS_GRIPPER_AVAILABLE:
return
self.gripperHandler(finger_value=-0.01,
finger_open=False,
finger_close=True)
@log
def executeTrajectory(self, trajectory, server):
goal = ExecuteTrajectoryGoal()
goal.trajectory = trajectory
self.execution_client_.send_goal(goal)
while self.execution_client_.get_state() < 3:
rospy.sleep(0.1)
if not server.is_preempt_requested() and not rospy.is_shutdown():
continue
self.execution_client_.cancel_goal()
self.execution_client_.wait_for_result()
return True
result = self.execution_client_.get_result()
state = self.execution_client_.get_state()
if result.success and state == 3:
rospy.logwarn("Execution finish with given time")
return True
else:
rospy.logerr("Execution aborted!!")
return False
@log
def isPoseAccessible(self, target_pose):
(_, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_,
object_pose=target_pose,
bdmp_goal=None,
bdmp_action='')
return is_planned
@log
def planAndExecuteTrajectoryInJointSpaces(self, target_joints, server):
(trajectory,
is_planned) = planTrajectoryInJointSpace(client=self.plan_client_,
object_pose=target_joints,
bdmp_goal=None,
bdmp_action='')
if not is_planned:
rospy.logerr('Trajectory in joint spaces execution failed')
raise Exception('Trajectory in joint spaces execution failed')
self.confirm()
is_execution_successful = self.executeTrajectory(trajectory=trajectory,
server=server)
if not is_execution_successful:
rospy.logerr('Can not find valid trajectory at joint space')
raise Exception('Trajectory Execution failed')
return not server.is_preempt_requested()
@log
def planAndExecuteTrajectoryInCartesianSpace(self, target_pose, server):
(trajectory,
is_planned) = planTrajectoryInCartSpace(client=self.plan_client_,
object_pose=target_pose,
bdmp_goal=None,
bdmp_action='')
if not is_planned:
rospy.logerr('Cannot find valid trajectory at cartesianSpace')
raise Exception('Cannot find valid trajectory at cartesian space')
self.confirm()
is_execution_successful = self.executeTrajectory(trajectory=trajectory,
server=server)
if not is_execution_successful:
rospy.logerr("Trajectory Execution Failed")
raise Exception('Trajectory Execution Failed')
return not server.is_preempt_requested()
@staticmethod
def poseToLists(pose):
position = [pose.position.x, pose.position.y, pose.position.z]
orientation = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
return (position, orientation)
@log
def catchTrashcanCallback(self, goal):
rospy.sleep(3)
result = MoveToResult()
result.arrived = False
target_pose = goal.target_pos
target_pose.pose.position.z *= 1.1
#The arm canot carry a new trashcan as it already carries one
if self.status_ != ArmStatus.NO_TRASHCAN:
self.catch_trashcan_server_.set_aborted(result)
try:
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.catch_trashcan_server_)
except Exception as e:
print(e)
self.catch_trashcan_server_.set_aborted(result)
return
if self.catch_trashcan_server_.is_preempt_requested():
result.arrived = True
self.catch_trashcan_server_.set_preempted(result)
return
self.closeGripper()
#self.fixTrashCanToRobot()
self.status_ = ArmStatus.FULL_TRASHCAN
# todo rmb-ma setup the grapping move with a real trashcan
# Help:
# try:
# target_pose = make_pose(position=[0.00,-0.00,-0.07], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
# if self.catch_trashcan_server_.is_preempt_requested():
# result.arrived = True
# self.catch_trashcan_server_.set_preempted(result)
# return
#
# try:
# target_pose = make_pose(position=[0.020,-0.00,0.00], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
# if self.catch_trashcan_server_.is_preempt_requested():
# result.arrived = True
# self.catch_trashcan_server_.set_preempted(result)
# return
#
# try:
# target_pose = make_pose(position=[0.000,-0.00,0.030], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
result.arrived = True
self.catch_trashcan_server_.set_succeeded(result)
@log
def emptyTrashcanCallback(self, goal):
result = MoveToResult()
#Trashcan emptying while the arm doesnt carry any trashcan
if self.status_ != ArmStatus.FULL_TRASHCAN:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
try:
self.planAndExecuteTrajectoryInCartesianSpace(
goal.target_pos, self.empty_trashcan_server_)
except:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
if self.empty_trashcan_server_.is_preempt_requested():
self.empty_trashcan_server_.set_preempted()
return
try:
target_joints = self.joint_values_[0:-1] + [3.]
self.planAndExecuteTrajectoryInJointSpaces(
target_joints, self.empty_trashcan_server_)
except:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
if self.empty_trashcan_server_.is_preempt_requested():
self.empty_trashcan_server_.set_preempted()
return
rospy.sleep(5)
self.status_ = ArmStatus.FULL_TRASHCAN
target_joints = self.joint_values_[0:-1] + [0]
self.planAndExecuteTrajectoryInJointSpaces(target_joints,
self.empty_trashcan_server_)
result.arrived = True
self.empty_trashcan_server_.set_succeeded(result)
@log
def moveToRestPositionCallback(self, goal):
result = MoveToResult()
# Rest position is unavailable if the arm carries a trashcan
if self.status_ != ArmStatus.NO_TRASHCAN:
result.arrived = False
self.to_rest_position_server_.set_aborted(result)
return
try:
self.planAndExecuteTrajectoryInJointSpaces(
self.DEFAULT_POSITION, self.to_rest_position_server_)
except Exception as e:
rospy.logerr(e)
result.arrived = False
self.to_rest_position_server_.set_aborted(result)
return
if self.to_rest_position_server_.is_preempt_requested():
self.to_rest_position_server_.set_preempted()
return
self.to_rest_position_server_.set_succeeded()
@log
def moveToTransportPositionCallback(self, goal):
result = MoveToResult()
# Transport position is unavailable if the arm doesnt carry a trashcan
if self.status_ == ArmStatus.NO_TRASHCAN:
result.arrived = False
self.to_transport_position_server_.set_aborted(result)
return
try:
result.arrived = self.planAndExecuteTrajectoryInJointSpaces(
self.TRANSPORT_POSITION, self.to_transport_position_server_)
except:
result.arrived = False
self.to_transport_position_server_.set_aborted(result)
return
if self.to_transport_position_server_.is_preempt_requested():
self.to_transport_position_server_.set_preempted()
return
self.to_transport_position_server_.set_succeeded(result)
@log
def leaveTrashcanCallback(self, goal):
result = MoveToResult()
# The arm cannot leave a trashcan if it doesnt carry one
if self.status_ != ArmStatus.EMPTY_TRASHCAN:
result.arrived = False
self.leave_trashcan_server_.set_aborted(result)
return
self.planAndExecuteTrajectoryInCartesianSpace(
goal.target_pos, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
self.openGripper()
self.status_ = ArmStatus.NO_TRASHCAN
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
target_pose = make_pose(position=[-0.07, -0., -0.04],
orientation=[0., 0., 0., 1.],
frame_id='gripper')
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
target_pose = make_pose(position=[-0.0, -0., 0.05],
orientation=[0., 0., 0., 1.],
frame_id='gripper')
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
result.arrived = True
self.leave_trashcan_server_.set_succeeded(result)
@log
def moveToJointsPositionCallback(self, goal):
target_joints = goal.trajectory.joint_trajectory.points[0].positions
self.planAndExecuteTrajectoryInJointSpaces(
target_joints, self.to_joints_position_server_)
if self.to_joints_position_server_.is_preempt_requested():
self.to_joints_position_server_.set_preempted()
return
self.to_joints_position_server_.set_succeeded()
class Approach(object):
def __init__(self, name, takeoff_height):
self.robot_name = name
self.takeoff_height = takeoff_height
# Mutual exclusion
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
# Create trajectory server
self.trajectory_server = SimpleActionServer(
'approach_server', ExecuteDroneApproachAction, self.goCallback,
False)
self.server_feedback = ExecuteDroneApproachFeedback()
self.server_result = ExecuteDroneApproachResult()
# Get client from hector_quadrotor_actions
self.move_client = SimpleActionClient("/{}/action/pose".format(name),
PoseAction)
self.move_client.wait_for_server()
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry,
self.poseCallback)
# Subscribe to topic to receive the planned trajectory
rospy.Subscriber("/{}/move_group/display_planned_path".format(name),
DisplayTrajectory, self.planCallback)
#Auxiliary variables
self.trajectory = [] # Array with the trajectory to be executed
self.trajectory_received = False # Flag to signal trajectory received
self.odom_received = False # Flag to signal odom received
self.robot = RobotCommander(
robot_description="{}/robot_description".format(name),
ns="/{}".format(name))
self.display_trajectory_publisher = rospy.Publisher(
'/{}/move_group/display_planned_path'.format(name),
DisplayTrajectory,
queue_size=20)
# Variables for collision callback
self.validity_srv = rospy.ServiceProxy(
'/{}/check_state_validity'.format(name), GetStateValidity)
self.validity_srv.wait_for_service()
self.collision = False
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
#Start move_group
self.move_group = MoveGroup('earth', name)
self.move_group.set_planner(planner_id='RRTConnectkConfigDefault',
attempts=10,
allowed_time=2) #RRTConnectkConfigDefault
self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX,
ZMAX]) # Set the workspace size
# Get current robot position to define as start planning point
self.current_pose = self.robot.get_current_state()
#Start planningScenePublisher
self.scene_pub = PlanningScenePublisher(name, self.current_pose)
# Start trajectory server
self.trajectory_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.notify()
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
# print(self.odometry)
self.odometry_me.release()
self.odom_received = True
def goCallback(self, pose):
'''
Require a plan to go to the desired target and try to execute it 5 time or return erro
'''
self.target = pose.goal
####################################################################
# First takeoff if the drone is close to ground
self.sonar_me.acquire()
while not (self.current_height and self.odometry):
self.sonar_me.wait()
if self.current_height < self.takeoff_height:
self.odometry_me.acquire()
takeoff_pose = PoseGoal()
takeoff_pose.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
takeoff_pose.target_pose.pose.position.x = self.odometry.position.x
takeoff_pose.target_pose.pose.position.y = self.odometry.position.y
takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height
takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.odometry_me.release()
self.move_client.send_goal(takeoff_pose)
self.move_client.wait_for_result()
result = self.move_client.get_state()
if result == GoalStatus.ABORTED:
rospy.logerr("Abort approach! Unable to execute takeoff!")
self.trajectory_server.set_aborted()
return
self.sonar_me.release()
####################################################################
rospy.loginfo("Try to start from [{},{},{}]".format(
self.odometry.position.x, self.odometry.position.y,
self.odometry.position.z))
rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x,
self.target.position.y,
self.target.position.z))
self.trials = 0
while self.trials < 5:
rospy.logwarn("Attempt {}".format(self.trials + 1))
if (self.trials > 1):
self.target.position.z += 2 * rd() - 1
result = self.go(self.target)
if (result == 'replan') or (result == 'no_plan'):
self.trials += 1
else:
self.trials = 10
self.collision = False
if result == 'ok':
self.trajectory_server.set_succeeded()
elif (result == 'preempted'):
self.trajectory_server.set_preempted()
else:
self.trajectory_server.set_aborted()
def go(self, target_):
'''
Function to plan and execute the trajectory one time
'''
# Insert goal position on an array
target = []
target.append(target_.position.x)
target.append(target_.position.y)
target.append(target_.position.z)
target.append(target_.orientation.x)
target.append(target_.orientation.y)
target.append(target_.orientation.z)
target.append(target_.orientation.w)
#Define target for move_group
# self.move_group.set_joint_value_target(target)
self.move_group.set_target(target)
self.odometry_me.acquire()
self.current_pose.multi_dof_joint_state.transforms[
0].translation.x = self.odometry.position.x
self.current_pose.multi_dof_joint_state.transforms[
0].translation.y = self.odometry.position.y
self.current_pose.multi_dof_joint_state.transforms[
0].translation.z = self.odometry.position.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.x
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.y
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.w
self.odometry_me.release()
#Set start state
self.move_group.set_start_state(self.current_pose)
# Plan a trajectory till the desired target
plan = self.move_group.plan()
if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory
while (not self.trajectory_received):
rospy.loginfo("Waiting for trajectory!")
rospy.sleep(0.2)
# rospy.loginfo("TRAJECTORY: {}".format(self.trajectory))
#Execute trajectory with action_pose
last_pose = self.trajectory[0]
for pose in self.trajectory:
# Verify preempt call
if self.trajectory_server.is_preempt_requested():
self.move_client.send_goal(last_pose)
self.move_client.wait_for_result()
self.scene_pub.publishScene()
self.trajectory_received = False
self.odom_received = False
return 'preempted'
#Send next pose to move
self.next_pose = pose.target_pose.pose
self.move_client.send_goal(pose,
feedback_cb=self.collisionCallback)
self.move_client.wait_for_result()
result = self.move_client.get_state()
self.scene_pub.publishScene()
# Abort if the drone can not reach the position
if result == GoalStatus.ABORTED:
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'aborted'
elif result == GoalStatus.PREEMPTED:
# last_pose.target_pose.pose.position.z += rd() - 0.5
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'replan'
elif result == GoalStatus.SUCCEEDED:
self.trials = 0
last_pose = pose
self.server_feedback.current_pose = self.odometry
self.trajectory_server.publish_feedback(self.server_feedback)
# Reset control variables
self.trajectory_received = False
self.odom_received = False
rospy.loginfo("Trajectory is traversed!")
return 'ok'
else:
rospy.logerr("Trajectory is empty. Planning was unsuccessful.")
return 'no_plan'
def planCallback(self, msg):
'''
Receive planned trajectories and insert it into an array of waypoints
'''
if (not self.odom_received):
return
# Variable to calculate the distance difference between 2 consecutive points
last_pose = PoseGoal()
last_pose.target_pose.pose.position.x = self.odometry.position.x
last_pose.target_pose.pose.position.y = self.odometry.position.y
last_pose.target_pose.pose.position.z = self.odometry.position.z
last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.trajectory = []
last_motion_theta = 0
for t in msg.trajectory:
for point in t.multi_dof_joint_trajectory.points:
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
# Orientate the robot always to the motion direction
delta_x = point.transforms[
0].translation.x - last_pose.target_pose.pose.position.x
delta_y = point.transforms[
0].translation.y - last_pose.target_pose.pose.position.y
motion_theta = atan2(delta_y, delta_x)
last_motion_theta = motion_theta
# Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION
# if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION):
q = quaternion_from_euler(0, 0, motion_theta)
waypoint.target_pose.pose.orientation.x = q[0]
waypoint.target_pose.pose.orientation.y = q[1]
waypoint.target_pose.pose.orientation.z = q[2]
waypoint.target_pose.pose.orientation.w = q[3]
# else:
# waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x
# waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y
# waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z
# waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w
# Add a rotation inplace if next position has an angle difference bigger than 45
if abs(motion_theta - last_motion_theta) > 0.785:
last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation
self.trajectory.append(last_pose)
last_pose = copy.copy(
waypoint) # Save pose to calc the naxt delta
last_motion_theta = motion_theta
self.trajectory.append(waypoint)
#Insert a last point to ensure that the robot end at the right position
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
waypoint.target_pose.pose.orientation.x = point.transforms[
0].rotation.x
waypoint.target_pose.pose.orientation.y = point.transforms[
0].rotation.y
waypoint.target_pose.pose.orientation.z = point.transforms[
0].rotation.z
waypoint.target_pose.pose.orientation.w = point.transforms[
0].rotation.w
self.trajectory.append(waypoint)
self.trajectory_received = True
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
class StateMachine(object):
def __init__(self):
self.node_name = "### Student SM"
### Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
# Custom params
self.cube_pose = [
float(x) for x in rospy.get_param(rospy.get_name() +
"/cube_pose").split(', ')
]
self.pickup_srv_nm = rospy.get_param(rospy.get_name() + '/pick_srv')
self.place_srv_nm = rospy.get_param(rospy.get_name() + '/place_srv')
self.marker_pose_topic = rospy.get_param(
'/robotics_intro/manipulation_client/marker_pose_topic')
### Subscribe to topics
# Custom topic subs
### Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
rospy.wait_for_service(self.pickup_srv_nm, timeout=30)
rospy.wait_for_service(self.place_srv_nm, timeout=30)
### Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
# Custom topic pubs
self.cube_pub = rospy.Publisher('/marker_pose_topic',
PoseStamped,
queue_size=10)
### Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
# Custom action setups
self.pickup_pose_ac = SimpleActionClient('/pickup_marker_pose',
PickUpPoseAction)
if not self.pickup_pose_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr('%s: Could not connect to pickup pose action server',
self.node_name)
exit()
rospy.loginfo('%s: Connected to pickup pose action server',
self.node_name)
# Init state machine
self.state = 1
rospy.sleep(3)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state > 0:
# State: Tuck arm
if self.state == 1:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
success_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(100.0))
if success_tucking:
rospy.loginfo("%s: Arm tuck: ",
self.play_motion_ac.get_result())
self.state = 2
else:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = -1
rospy.sleep(1)
# State: Lower robot head service
if self.state == 2:
try:
rospy.loginfo("%s: Lowering robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
self.state = 3
rospy.loginfo("%s: Move head down succeded!",
self.node_name)
else:
rospy.loginfo("%s: Move head down failed!",
self.node_name)
self.state = -1
rospy.sleep(1)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
# State: Pickup Cube
if self.state == 3:
rospy.loginfo('%s: Pickup Cube...', self.node_name)
## define pose msg
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time()
pose_msg.header.frame_id = 'base_footprint'
pose_msg.pose.position.x = self.cube_pose[0] + 0.02
pose_msg.pose.position.y = self.cube_pose[1]
pose_msg.pose.position.z = self.cube_pose[2] - 0.05
pose_msg.pose.orientation.x = self.cube_pose[3]
pose_msg.pose.orientation.y = self.cube_pose[4]
pose_msg.pose.orientation.z = self.cube_pose[5]
pose_msg.pose.orientation.w = self.cube_pose[6]
#pickuppose_goal = PickUpPoseGoal()
#pickuppose_goal.object_pose = pose_msg
self.cube_pub.publish(pose_msg)
pickup_srv = rospy.ServiceProxy(self.pickup_srv_nm, SetBool)
pickup_req = pickup_srv()
if pickup_req.success == True:
self.state = 4
rospy.loginfo("%s: Pickup SRV succeded!", self.node_name)
else:
rospy.loginfo("%s: Pickup SRV failed!", self.node_name)
self.state = -1
#self.pickup_pose_ac.send_goal(pickuppose_goal)
#success = self.pickup_pose_ac.wait_for_result(rospy.Duration(100.0))
#if success:
# rospy.loginfo('%s: State 0: Pickup Cube was successfull: %s', self.node_name, self.pickup_pose_ac.get_result())
#else:
# rospy.loginfo('%s: State 0: Pickup Cube was NOT successfull', self.node_name)
# Next state
#self.state = -2
# State: Turn the robot
if self.state == 4:
rospy.loginfo("%s: Turning", self.node_name)
move_msg = Twist()
move_msg.angular.z = -1
rate = rospy.Rate(10)
converged = False
cnt = 0
while not rospy.is_shutdown() and cnt < 30:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 5
rospy.sleep(1)
# State: Move to 2nd table
if self.state == 5:
rospy.loginfo("%s: Moving towards table", self.node_name)
move_msg = Twist()
move_msg.linear.x = 0.5
rate = rospy.Rate(10)
converged = False
cnt = 0
while not rospy.is_shutdown() and cnt < 17:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 6
rospy.sleep(1)
# State: Drop the cube
if self.state == 6:
rospy.loginfo("%s: Dropping the cube...", self.node_name)
try:
drop_cube = rospy.ServiceProxy(self.place_srv_nm, SetBool)
drop_cube_request = drop_cube(True)
if drop_cube_request.success == True:
rospy.loginfo("%s: Placed cube", self.node_name)
self.state = -2
else:
rospy.loginfo("%s: failed to place the cube",
self.node_name)
self.state = -1
rospy.sleep(1)
except rospy.ServiceException, e:
print "Service call to place server failed: %s" % e
# State 6: Home
if self.state == 7:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
success_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(100.0))
if success_tucking:
rospy.loginfo("%s: Arm Tucked.", self.node_name)
self.state = -2
else:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = -1
rospy.sleep(1)
# Error handling
if self.state == -1:
rospy.logerr(
"%s: State machine failed. Check your code and try again!",
self.node_name)
return
class Arm(object):
def __init__(self):
# Client
self.arm_client = SimpleActionClient(ARM_CLIENT_TOPIC,
FollowJointTrajectoryAction)
rospy.loginfo("ARM CLIENT: Waiting for arm action server...")
arm_client_running = self.arm_client.wait_for_server(rospy.Duration(2))
if arm_client_running:
rospy.loginfo("ARM CLIENT: Arm controller initialized.")
else:
rospy.loginfo("ARM CLIENT: Arm controller is NOT initialized!")
# Subscribe for states
rospy.Subscriber(ARM_STATE_TOPIC, JointTrajectoryControllerState,
self._update_state)
self.curr_arm_state_ = JointTrajectoryControllerState()
# Setup
self.arm_goal = FollowJointTrajectoryGoal()
self.arm_goal.trajectory.joint_names = ARM_JOINTS
self.p = JointTrajectoryPoint()
self.timeout = rospy.Duration(30)
# Pre-def. positions
r = RosPack()
self.yaml_filename = r.get_path(
'frasier_utilities') + '/config/arm_configs.yaml'
with open(self.yaml_filename, 'r') as stream:
try:
self.positions = yaml.load(stream)
except yaml.YAMLError as exc:
rospy.logwarn(
"ARM CLIENT: Yaml Exception Caught: {}".format(exc))
rospy.logdebug("ARM CLIENT: Config: {}".format(self.positions))
def _update_state(self, msg):
self.curr_arm_state_ = msg
def _save_yaml_file(self):
with open(self.yaml_filename, 'w') as outfile:
yaml.dump(self.positions, outfile, default_flow_style=False)
def _read_yaml_file(self):
with open(self.yaml_filename, 'r') as outfile:
try:
self.positions = yaml.load(outfile)
except yaml.YAMLError as e:
print e
def _send_goal(self, arm_goal=None, blocking=False):
if arm_goal is None:
self.arm_client.send_goal(self.arm_goal)
else:
self.arm_client.send_goal(arm_goal)
if blocking:
rospy.loginfo("ARM CLIENT: Waiting for goal to complete...")
result = self.arm_client.wait_for_result(self.timeout)
if not result:
rospy.logwarn("ARM CLIENT: Goal timed out, canceled!")
self.arm_client.cancel_goal()
return False
else:
state = self.arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("ARM CLIENT: Goal completed.")
return True
else:
rospy.logwarn("ARM CLIENT: Goal failed!")
return False
# Assume it succeeded
return True
def reset(self):
self.goto_position('start', True)
def save_position(self,
position_name='default',
arm_position=None,
save_file=True):
if arm_position is None:
self.positions[position_name] = list(
self.curr_arm_state_.actual.positions)
else:
if not isinstance(arm_position, list):
raise TypeError("Arm positions must be of type list!")
self.positions[position_name] = arm_position
if save_file:
self._save_yaml_file()
return True
def goto_position(self, position=None, blocking=False):
self._read_yaml_file()
# Goto yaml file position if string
if isinstance(position, str):
if position in self.positions:
self.p.positions = self.positions[position]
self.p.time_from_start = rospy.Time(3)
self.arm_goal.trajectory.points = [self.p]
return self._send_goal()
else:
rospy.logwarn(
"ARM CLIENT: {} does not exist in the config file!".format(
position))
elif isinstance(position, list):
self.arm_goal.trajectory.points = position
return self._send_goal(self.arm_goal, blocking)
elif position is None:
return self._send_goal(self.arm_goal, blocking)
def __del__(self):
rospy.loginfo("Shutting down arm client and saving locations")
self._save_yaml_file()
class GiskardWrapper(object):
def __init__(self, node_name=u'giskard'):
giskard_topic = u'{}/command'.format(node_name)
if giskard_topic is not None:
self._client = SimpleActionClient(giskard_topic, MoveAction)
self._update_world_srv = rospy.ServiceProxy(
u'{}/update_world'.format(node_name), UpdateWorld)
self._get_object_names_srv = rospy.ServiceProxy(
u'{}/get_object_names'.format(node_name), GetObjectNames)
self._get_object_info_srv = rospy.ServiceProxy(
u'{}/get_object_info'.format(node_name), GetObjectInfo)
self._update_rviz_markers_srv = rospy.ServiceProxy(
u'{}/update_rviz_markers'.format(node_name), UpdateRvizMarkers)
self._get_attached_objects_srv = rospy.ServiceProxy(
u'{}/get_attached_objects'.format(node_name),
GetAttachedObjects)
self._marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(node_name))
self._client.wait_for_server()
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self._object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
"""
:rtype: str
"""
return self.robot_urdf.get_name()
def get_root(self):
"""
Returns the name of the robot's root link
:rtype: str
"""
return self.robot_urdf.get_root()
def get_robot_links(self):
"""
Returns a list of the robots links
:rtype: dict
"""
return self.robot_urdf.get_link_names()
def get_joint_states(self, topic=u'joint_states', timeout=1):
"""
Returns a dictionary of all joints (key) and their position (value)
:param topic: joint_state topic
:param timeout: duration to wait for JointState msg
:return: OrderedDict[str, float]
"""
try:
msg = rospy.wait_for_message(topic, JointState,
rospy.Duration(timeout))
return to_joint_state_position_dict(msg)
except rospy.ROSException:
rospy.logwarn("get_joint_states: wait_for_message timeout")
return {}
def set_cart_goal(self,
root_link,
tip_link,
goal_pose,
max_linear_velocity=None,
max_angular_velocity=None,
weight=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal pose
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose
:type goal_pose: PoseStamped
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_translation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_linear_velocity,
weight=weight)
self.set_rotation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_angular_velocity,
weight=weight)
def set_translation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal position
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, orientation will be ignored
:type goal_pose: PoseStamped
:param max_velocity: m/s, default 0.1
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal orientation
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, position will be ignored
:type goal_pose: PoseStamped
:param max_velocity: rad/s, default 0.5
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, goal_state, weight=None, max_velocity=None):
"""
This goal will move the robots joint to the desired configuration.
:param goal_state: Can either be a joint state messages or a dict mapping joint name to position.
:type goal_state: Union[JointState, dict]
:param weight: default WEIGHT_BELOW_CA
:type weight: float
:param max_velocity: default is the default of the added joint goals
:type max_velocity: float
"""
if weight is None and max_velocity is None:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
constraint.goal_state = goal_state
else:
for joint_name, joint_position in goal_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
self.cmd_seq[-1].joint_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
goal_state = goal_state
else:
goal_state2 = JointState()
for joint_name, joint_position in goal_state.items():
goal_state2.name.append(joint_name)
goal_state2.position.append(joint_position)
goal_state = goal_state2
params = {}
params[u'goal_state'] = convert_ros_message_to_dictionary(
goal_state)
if weight is not None:
params[u'weight'] = weight
if max_velocity is not None:
params[u'max_velocity'] = max_velocity
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self,
tip_link,
tip_normal,
root_link=None,
root_normal=None,
max_angular_velocity=None,
weight=WEIGHT_ABOVE_CA):
"""
This Goal will use the kinematic chain between tip and root normal to align both
:param root_link: name of the root link for the kinematic chain, default robot root link
:type root_link: str
:param tip_link: name of the tip link for the kinematic chain
:type tip_link: str
:param tip_normal: normal at the tip of the kin chain, default is z axis of robot root link
:type tip_normal: Vector3Stamped
:param root_normal: normal at the root of the kin chain
:type root_normal: Vector3Stamped
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
if root_link is None:
root_link = self.get_root()
if root_normal is None:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
params = {
u'tip_link': tip_link,
u'tip_normal': tip_normal,
u'root_link': root_link,
u'root_normal': root_normal
}
if weight is not None:
params[u'weight'] = weight
if max_angular_velocity is not None:
params[u'max_angular_velocity'] = max_angular_velocity
self.set_json_goal(u'AlignPlanes', **params)
def avoid_joint_limits(self, percentage=15, weight=WEIGHT_BELOW_CA):
"""
This goal will push joints away from their position limits
:param percentage: default 15, if limits are 0-100, the constraint will push into the 15-85 range
:type percentage: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
self.set_json_goal(u'AvoidJointLimits',
percentage=percentage,
weight=weight)
def limit_cartesian_velocity(self,
root_link,
tip_link,
weight=WEIGHT_ABOVE_CA,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
hard=True):
"""
This goal will limit the cartesian velocity of the tip link relative to root link
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param hard: default True, will turn this into a hard constraint, that will always be satisfied, can could
make some goal combination infeasible
:type hard: bool
"""
self.set_json_goal(u'CartesianVelocityLimit',
root_link=root_link,
tip_link=tip_link,
weight=weight,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
hard=hard)
def grasp_bar(self,
root_link,
tip_link,
tip_grasp_axis,
bar_center,
bar_axis,
bar_length,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
weight=WEIGHT_ABOVE_CA):
"""
This goal can be used to grasp bars. It's like a cartesian goal with some freedom along one axis.
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param tip_grasp_axis: this axis of the tip will be aligned with bar_axis
:type tip_grasp_axis: Vector3Stamped
:param bar_center: center of the bar
:type bar_center: PointStamped
:param bar_axis: tip_grasp_axis will be aligned with this vector
:type bar_axis: Vector3Stamped
:param bar_length: length of the bar
:type bar_length: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_json_goal(u'GraspBar',
root_link=root_link,
tip_link=tip_link,
tip_grasp_axis=tip_grasp_axis,
bar_center=bar_center,
bar_axis=bar_axis,
bar_length=bar_length,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
weight=weight)
def set_pull_door_goal(self,
tip_link,
object_name_prefix,
object_link_name,
angle_goal,
weight=WEIGHT_ABOVE_CA):
"""
:type tip_link: str
:param tip_link: tip of manipulator (gripper) which is used
:type object_name_prefix: object name link prefix
:param object_name_prefix: string
:type object_link_name str
:param object_link_name name of the object link name
:type object_link_name str
:param object_link_name handle to grasp
:type angle_goal: float
:param angle_goal: how far to open
:type weight float
:param weight Default = WEIGHT_ABOVE_CA
"""
self.set_json_goal(u'OpenDoor',
tip_link=tip_link,
object_name=object_name_prefix,
object_link_name=object_link_name,
angle_goal=angle_goal,
weight=weight)
def update_god_map(self, updates):
"""
don't use, it's only for hacks :)
"""
self.set_json_goal(u'UpdateGodMap', updates=updates)
def pointing(self,
tip_link,
goal_point,
root_link=None,
pointing_axis=None,
weight=None):
"""
Uses the kinematic chain from root_link to tip_link to move the pointing axis, such that it points to the goal point.
:param tip_link: name of the tip of the kin chain
:type tip_link: str
:param goal_point: where the pointing_axis will point towards
:type goal_point: PointStamped
:param root_link: name of the root of the kin chain
:type root_link: str
:param pointing_axis: default is z axis, this axis will point towards the goal_point
:type pointing_axis: Vector3Stamped
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
kwargs = {u'tip_link': tip_link, u'goal_point': goal_point}
if root_link is not None:
kwargs[u'root_link'] = root_link
if pointing_axis is not None:
kwargs[u'pointing_axis'] = pointing_axis
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = goal_point
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
"""
Set a goal for any of the goals defined in Constraints.py
:param constraint_type: Name of the Goal
:type constraint_type: str
:param kwargs: maps constraint parameter names to values. Values should be float, str or ros messages.
:type kwargs: dict
"""
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
"""
Adds collision entries to the current goal
:param collisions: list of CollisionEntry
:type collisions: list
"""
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self,
min_dist,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
"""
Allows the collision with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_self_collision(self):
"""
Avoid collisions with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal. The distance will override anything from the config file.
If you don't want to override the distance, don't call this function. Avoid all is the default, if you don't
add any collision entries.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self):
"""
Adds another command to the goal sequence. Any set goal calls will be added the this new goal.
This is used, if you want Giskard to plan multiple goals in succession.
"""
move_cmd = MoveCmd()
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_AND_EXECUTE, wait)
def check_reachability(self, wait=True):
"""
Not implemented
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.CHECK_REACHABILITY, wait)
def plan(self, wait=True):
"""
Plans, but doesn't execute the goal. Useful, if you just want to look at the planning ghost.
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_ONLY, wait)
def send_goal(self, goal_type, wait=True):
goal = self._get_goal()
goal.type = goal_type
if wait:
self._client.send_goal_and_wait(goal)
return self._client.get_result()
else:
self._client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
"""
Stops any goal that Giskard is processing.
"""
self._client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self._client.wait_for_result(timeout)
return self._client.get_result()
def clear_world(self):
"""
Removes any objects and attached objects from Giskard's world and reverts the robots urdf to what it got from
the parameter server.
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(),
False, PoseStamped())
return self._update_world_srv.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False,
PoseStamped())
return self._update_world_srv.call(req)
def add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: (x length, y length, z length) in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self._update_world_srv.call(req)
def add_sphere(self,
name=u'sphere',
size=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: radius in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_mesh(self,
name=u'mesh',
mesh=u'',
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param mesh: path to the meshes location. e.g. package://giskardpy/test/urdfs/meshes/bowl_21.obj
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param height: in m
:type height: float
:param radius: in m
:type radius: float
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions = [0, 0]
object.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = height
object.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS] = radius
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
pose=None):
"""
Add a box to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:param pose: pose of the box
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self._update_world_srv.call(req)
def attach_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=None,
position=None,
orientation=None):
"""
Add a cylinder to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type height: int
:param height: height of the cylinder. Default = 1
:type radius: int
:param radius: radius of the cylinder. Default = 1
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self._update_world_srv.call(req)
def attach_object(self, name, link_frame_id):
"""
Attach an already existing object at link_frame_id of the robot.
:type name: str
:param link_frame_id: name of a robot link
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self._update_world_srv.call(req)
def detach_object(self, object_name):
"""
Detach an object from the robot and add it back to the world.
Careful though, you could amputate an arm be accident!
:type object_name: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self._update_world_srv.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
"""
Adds a urdf to the world
:param name: name it will have in the world
:type name: str
:param urdf: urdf as string, no path
:type urdf: str
:type pose: PoseStamped
:param js_topic: Giskard will listen on that topic for joint states and update the urdf accordingly
:type js_topic: str
:param set_js_topic: A topic that the python wrapper will use to set the urdf joint state.
If None, set_js_topic == js_topic
:type set_js_topic: str
:return: UpdateWorldResponse
"""
if set_js_topic is None:
set_js_topic = js_topic
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self._object_js_topics[name] = rospy.Publisher(set_js_topic,
JointState,
queue_size=10)
return self._update_world_srv.call(req)
def set_object_joint_state(self, object_name, joint_states):
"""
:type object_name: str
:param joint_states: joint state message or a dict that maps joint name to position
:type joint_states: Union[JointState, dict]
:return: UpdateWorldResponse
"""
if isinstance(joint_states, dict):
joint_states = position_dict_to_joint_states(joint_states)
self._object_js_topics[object_name].publish(joint_states)
def get_object_names(self):
"""
returns the names of every object in the world
:rtype: GetObjectNamesResponse
"""
return self._get_object_names_srv()
def get_object_info(self, name):
"""
returns the joint state, joint state topic and pose of the object with the given name
:type name: str
:rtype: GetObjectInfoResponse
"""
return self._get_object_info_srv(name)
def update_rviz_markers(self, object_names):
"""
republishes visualization markers for rviz
:type object_names: list
:rtype: UpdateRvizMarkersResponse
"""
return self._update_rviz_markers_srv(object_names)
def get_attached_objects(self):
"""
returns a list of all objects that are attached to the robot and the respective attachement points
:rtype: GetAttachedObjectsResponse
"""
return self._get_attached_objects_srv()
class StateMachine(object):
def __init__(self):
self.node_name = "Student SM"
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() + '/move_head_srv')
# Subscribe to topics
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top, Twist, queue_size=10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...", self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion", PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server", self.node_name)
# Init state machine
self.state = 0
rospy.sleep(3)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state != 4:
# State 0: Move the robot "manually" to door
if self.state == 0:
move_msg = Twist()
move_msg.linear.x = 1
rate = rospy.Rate(10)
converged = False
cnt = 0
rospy.loginfo("%s: Moving towards door", self.node_name)
while not rospy.is_shutdown() and cnt < 25:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 1
rospy.sleep(1)
# State 1: Tuck arm
if self.state == 1:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
success_tucking = self.play_motion_ac.wait_for_result(rospy.Duration(100.0))
if success_tucking:
rospy.loginfo("%s: Arm tuck: ", self.play_motion_ac.get_result())
self.state = 2
else:
self.play_motion_ac.cancel_goal()
rospy.logerr("%s: play_motion failed to tuck arm, reset simulation", self.node_name)
self.state = 5
rospy.sleep(1)
# State 2: Move the robot "manually" to chair
if self.state == 2:
move_msg = Twist()
move_msg.angular.z = -1
rate = rospy.Rate(10)
converged = False
cnt = 0
rospy.loginfo("%s: Moving towards table", self.node_name)
while not rospy.is_shutdown() and cnt < 5:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
move_msg.linear.x = 1
move_msg.angular.z = 0
cnt = 0
while not rospy.is_shutdown() and cnt < 15:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 3
rospy.sleep(1)
# State 3: Lower robot head service
if self.state == 3:
try:
rospy.loginfo("%s: Lowering robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm, MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
self.state = 4
rospy.loginfo("%s: Move head down succeded!", self.node_name)
else:
rospy.loginfo("%s: Move head down failed!", self.node_name)
self.state = 5
rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s"%e
# Error handling
if self.state == 5:
rospy.logerr("%s: State machine failed. Check your code and try again!", self.node_name)
return
rospy.loginfo("%s: State machine finished!", self.node_name)
return
class PathExecutor:
goal_index = 0
reached_all_nodes = True
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def handle_path(self, paramExecutePathGoal):
'''
Action server callback to handle following path in succession
'''
rospy.loginfo('handle_path')
self.goal_index = 0
self.executePathGoal = paramExecutePathGoal
self.executePathResult = ExecutePathResult()
if self.executePathGoal is not None:
self.visualization_publisher.publish(self.executePathGoal.path)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
if not self.path_server.is_active():
return
if self.path_server.is_preempt_requested():
rospy.loginfo('Preempt requested...')
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_preempted()
return
if self.goal_index < len(self.executePathGoal.path.poses):
moveto_goal = MoveToGoal()
moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
feedback_cb=self.handle_goal_preempt)
# Wait for result
while self.goal_client.get_result() is None:
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
else:
rospy.loginfo('Done processing goals')
self.goal_client.cancel_goal()
self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
return
rate.sleep()
self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def handle_goal(self, state, result):
'''
Handle goal events (succeeded, preempted, aborted, unreachable, ...)
Send feedback message
'''
feedback = ExecutePathFeedback()
feedback.pose = self.executePathGoal.path.poses[self.goal_index]
# state is GoalStatus message as shown here:
# http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(True)
feedback.reached = True
else:
rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(False)
feedback.reached = False
if not self.executePathGoal.skip_unreachable:
rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_succeeded(self.executePathResult,
'Unreachable goal in path. Done processing goals.')
#self.path_server.set_preempted()
#return
self.path_server.publish_feedback(feedback)
self.goal_index = self.goal_index + 1
def handle_goal_preempt(self, state):
'''
Callback for goal_client to check for preemption from path_server
'''
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
class GiskardWrapper(object):
def __init__(self, giskard_topic=u'giskardpy/command', ns=u'giskard'):
if giskard_topic is not None:
self.client = SimpleActionClient(giskard_topic, MoveAction)
self.update_world = rospy.ServiceProxy(u'{}/update_world'.format(ns), UpdateWorld)
self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(ns))
self.client.wait_for_server()
self.tip_to_root = {}
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self.object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
return self.robot_urdf.get_name()
def get_root(self):
return self.robot_urdf.get_root()
def set_cart_goal(self, root, tip, pose_stamped, trans_max_speed=None, rot_max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
self.set_translation_goal(root, tip, pose_stamped, max_speed=trans_max_speed)
self.set_rotation_goal(root, tip, pose_stamped, max_speed=rot_max_speed)
def set_translation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, joint_state, weight=None, gain=None, max_speed=None):
"""
:param joint_state:
:type joint_state: dict
"""
if not weight and not gain and not max_speed:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
elif isinstance(joint_state, JointState):
constraint.goal_state = joint_state
self.cmd_seq[-1].joint_constraints.append(constraint)
elif isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint = Constraint()
constraint.type = u'JointPosition'
params = {}
params[u'joint_name'] = joint_name
params[u'goal'] = joint_position
if weight:
params[u'weight'] = weight
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self, tip, tip_normal, root=None, root_normal=None):
"""
:type tip: str
:type tip_normal: Vector3Stamped
:type root: str
:type root_normal: Vector3Stamped
:return:
"""
root = root if root else self.get_root()
tip_normal = convert_ros_message_to_dictionary(tip_normal)
if not root_normal:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
root_normal = convert_ros_message_to_dictionary(root_normal)
self.set_json_goal(u'AlignPlanes', tip=tip, tip_normal=tip_normal, root=root, root_normal=root_normal)
def gravity_controlled_joint(self, joint_name, object_name):
self.set_json_goal(u'GravityJoint', joint_name=joint_name, object_name=object_name)
def update_god_map(self, updates):
self.set_json_goal(u'UpdateGodMap', updates=updates)
def update_yaml(self, updates):
self.set_json_goal(u'UpdateYaml', updates=updates)
def pointing(self, tip, goal_point, root=None, pointing_axis=None, weight=None):
kwargs = {u'tip':tip,
u'goal_point':goal_point}
if root is not None:
kwargs[u'root'] = root
if pointing_axis is not None:
kwargs[u'pointing_axis'] = convert_ros_message_to_dictionary(pointing_axis)
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = convert_ros_message_to_dictionary(goal_point)
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL,)):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self, min_dist, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL,)):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def set_self_collision_distance(self, min_dist=0.05):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = min_dist
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self, max_trajectory_length=20):
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = max_trajectory_length
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
goal = self._get_goal()
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def plan(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
goal = self._get_goal()
goal.type = MoveGoal.PLAN_ONLY
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
self.client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self.client.wait_for_result(timeout)
return self.client.get_result()
def clear_world(self):
"""
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped())
return self.update_world.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False, PoseStamped())
return self.update_world.call(req)
def add_box(self, name=u'box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
"""
:param name:
:param size: (x length, y length, z length)
:param frame_id:
:param position:
:param orientation:
:param pose:
:return:
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self.update_world.call(req)
def add_sphere(self, name=u'sphere', size=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_mesh(self, name=u'mesh', mesh=u'', frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_cylinder(self, name=u'cylinder', size=(1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions.append(size[0])
object.shape.dimensions.append(size[1])
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self.update_world.call(req)
def attach_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=None, position=None, orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self.update_world.call(req)
def attach_object(self, name, link_frame_id):
"""
:type name: str
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self.update_world.call(req)
def detach_object(self, object_name):
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self.update_world.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u''):
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False, pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self.object_js_topics[name] = rospy.Publisher(js_topic, JointState, queue_size=10)
return self.update_world.call(req)
def set_object_joint_state(self, object_name, joint_states):
if isinstance(joint_states, dict):
joint_states = dict_to_joint_states(joint_states)
self.object_js_topics[object_name].publish(joint_states)
class PathPlannerNode(object):
"""
This is a ROS node that is responsible for planning and executing
the a path through the field.
"""
def __init__(self):
# Setup ROS node
rospy.init_node('path_planner')
# ROS params
self.cut_spacing = rospy.get_param("~coverage_spacing", 0.4)
# Setup publishers and subscribers
rospy.Subscriber('heatmap_area', PolygonStamped, self.field_callback)
self.path_marker_pub = rospy.Publisher('visualization_marker',
MarkerArray,
latch=True)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
# Setup initial variables
self.field_shape = None
self.field_frame_id = None
self.path = None
self.path_status = None
self.path_markers = None
self.start_path_following = False
self.robot_pose = None
self.goal_state = None
self.current_destination = None
self.testing = False
self.current_distance = None
self.previous_destination = None
self.clear_costmaps = rospy.ServiceProxy('move_base/clear_costmaps', Empty)
self.just_reset = False
self.timeout = False
self.heading = 0
# Spin until shutdown or we are ready for path following
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
rate.sleep()
if self.start_path_following:
# Run until stopped
# Setup path following
self.setup_path_following(self.heading)
# Iterate on path following
while not rospy.is_shutdown():
if not self.step_path_following():
break
self.start_path_following = False
def field_callback(self, msg):
"""
Handles new field polygons, has to be called
at least once before planning happens.
"""
# Convert the PolygonStamped into a shapely polygon
temp_points = []
for point in msg.polygon.points:
temp_points.append( (float(point.x), float(point.y)) )
self.field_shape = geo.Polygon(temp_points)
self.field_frame_id = msg.header.frame_id
self.start_path_following = True
def odom_callback(self, msg):
"""
Watches for the robot's Odometry data, which is used in the path
planning as the initial robot position.
"""
self.robot_pose = msg
def plan_path(self, field_polygon, origin=None, degrees=0):
"""
This is called after a field polygon has been received.
This uses the automow_planning.coverage module to plan a
coverage path using the field geometry. The path consists of
a series of waypoints.
"""
# Get the rotation to align with the longest edge of the polygon
from maptools import rotation_tf_from_longest_edge, RotationTransform
rotation = rotation_tf_from_longest_edge(field_polygon)
rotation = RotationTransform(rotation.angle + degrees)
# Rotate the field polygon
print "Rotate the field polygon"
from maptools import rotate_polygon_from, rotate_polygon_to
transformed_field_polygon = rotate_polygon_from(field_polygon, rotation)
# Decompose the rotated field into a series of waypoints
from coverage import decompose
print origin
if origin is not None:
point_mat = np.mat([[origin[0], origin[1], 0]], dtype='float64').transpose()
origin = rotation.irm * point_mat
origin = (origin[0,0], origin[1,0])
print(self.cut_spacing)
transformed_path = decompose(transformed_field_polygon,
origin=(origin[0], origin[1]),
width=self.cut_spacing)
# Rotate the transformed path back into the source frame
from maptools import rotate_from, rotate_to
self.path = rotate_to(np.array(transformed_path), rotation)
# Calculate headings and extend the waypoints with them
self.path = self.calculate_headings(self.path, rotation)
# Set the path_status to 'not_visited'
self.path_status = []
for waypoint in self.path:
self.path_status.append('not_visited')
# Visualize the data
self.visualize_path(self.path, self.path_status)
def calculate_headings(self, path, rotation):
"""
Calculates the headings between paths and adds them to the waypoints.
"""
new_path = []
for index, waypoint in enumerate(path):
new_path.append(list(path[index]))
# If the end, copy the previous heading
if index == 0:
new_path[index].append(0)
continue
# Calculate the angle between this waypoint and the next
dx = path[index][0] - path[index-1][0]
dy = path[index][1] - path[index-1][1]
from math import atan2, pi
#if you want to use turning path, use atan
#heading = atan2(dy, dx)
heading = rotation.w + 1.5708
new_path[index].append(heading)
return new_path
def visualize_path(self, path, path_status=None):
"""
Convenience function, calls both visualize_path{as_path, as_marker}
"""
# TODO: finish this (path as path viz)
# self.visualize_path_as_path(path, path_status)
self.visualize_path_as_marker(path, path_status)
def visualize_path_as_path(self, path, path_status=None):
"""
Publishes a nav_msgs/Path msg containing the planned path.
If path_status is not None then the only waypoints put in the
nav_msgs/Path msg will be ones that are 'not_visited' or 'visiting'.
"""
now = rospy.Time.now()
msg = Path()
msg.header.stamp = now
msg.header.frame_id = self.field_frame_id
for index, waypoint in enumerate(path):
# Only put 'not_visited', 'visiting', and the most recent 'visited'
# in the path msg
if path_status != None: # If not set, ignore
if path_status[index] == 'visited': # if this one is visited
try:
# if the next one is visited too
if path_status[index+1] == 'visited':
# Then continue, because this one doesn't belong
# in the path msg
continue
except KeyError as e: # incase index+1 is too big
pass
# Otherwise if belongs, put it in
pose_msg = PoseStamped()
pose_msg.header.stamp = now
pose_msg.header.frame_id = self.field_frame_id
pose_msg.pose.position.x = waypoint[0]
pose_msg.pose.position.y = waypoint[1]
msg.poses.append(pose_msg)
def visualize_path_as_marker(self, path, path_status):
"""
Publishes visualization Markers to represent the planned path.
Publishes the path as a series of spheres connected by lines.
The color of the spheres is set by the path_status parameter,
which is a list of strings of which the possible values are in
['not_visited', 'visiting', 'visited'].
"""
# Get the time
now = rospy.Time.now()
# If self.path_markers is None, initialize it
if self.path_markers == None:
self.path_markers = MarkerArray()
# # If there are existing markers, delete them
# markers_to_delete = MarkerArray()
# if len(self.path_markers.markers) > 0:
# for marker in self.path_markers.markers:
# marker.action = Marker.DELETE
# markers_to_delete.markers.append(marker)
# self.path_marker_pub.publish(markers_to_delete)
# Clear the path_markers
self.path_markers = MarkerArray()
line_strip_points = []
# Create the waypoint markers
for index, waypoint in enumerate(path):
waypoint_marker = Marker()
waypoint_marker.header.stamp = now
waypoint_marker.header.frame_id = self.field_frame_id
waypoint_marker.ns = "waypoints"
waypoint_marker.id = index
waypoint_marker.type = Marker.ARROW
if index == 0:
waypoint_marker.type = Marker.CUBE
waypoint_marker.action = Marker.MODIFY
waypoint_marker.scale.x = 1
waypoint_marker.scale.y = 1
waypoint_marker.scale.z = 0.25
point = Point(waypoint[0], waypoint[1], 0)
waypoint_marker.pose.position = point
# Store the point for the line_strip marker
line_strip_points.append(point)
# Set the heading of the ARROW
quat = qfe(0, 0, waypoint[2])
waypoint_marker.pose.orientation.x = quat[0]
waypoint_marker.pose.orientation.y = quat[1]
waypoint_marker.pose.orientation.z = quat[2]
waypoint_marker.pose.orientation.w = quat[3]
# Color is based on path_status
status = path_status[index]
if status == 'not_visited':
waypoint_marker.color = ColorRGBA(1,0,0,0.5)
elif status == 'visiting':
waypoint_marker.color = ColorRGBA(0,1,0,0.5)
elif status == 'visited':
waypoint_marker.color = ColorRGBA(0,0,1,0.5)
else:
rospy.err("Invalid path status.")
waypoint_marker.color = ColorRGBA(1,1,1,0.5)
# Put this waypoint Marker in the MarkerArray
self.path_markers.markers.append(waypoint_marker)
# Create the line_strip Marker which connects the waypoints
line_strip = Marker()
line_strip.header.stamp = now
line_strip.header.frame_id = self.field_frame_id
line_strip.ns = "lines"
line_strip.id = 0
line_strip.type = Marker.LINE_STRIP
line_strip.action = Marker.ADD
line_strip.scale.x = 0.1
line_strip.color = ColorRGBA(0,0,1,0.5)
line_strip.points = line_strip_points
self.path_markers.markers.append(line_strip)
# Publish the marker array
self.path_marker_pub.publish(self.path_markers)
def setup_path_following(self, degrees=0):
"""
Sets up the node for following the planned path.
Will wait until the initial robot pose is set and until
the move_base actionlib service is available.
"""
# Create the actionlib service
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
connected_to_move_base = False
dur = rospy.Duration(1.0)
# If testing prime the robot_pose
if self.testing:
self.robot_pose = Odometry()
self.robot_pose.pose.pose.position.x = 0
self.robot_pose.pose.pose.position.y = 0
# Wait for the field shape
while self.field_shape == None:
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Print message about the waiting
msg = "Qualification: waiting on the field shape."
rospy.loginfo(msg)
rospy.Rate(1.0).sleep()
# Wait for the robot position
while self.robot_pose == None:
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Print message about the waiting
msg = "Qualification: waiting on initial robot pose."
rospy.loginfo(msg)
rospy.Rate(1.0).sleep()
# Now we should plan a path using the robot's initial pose
origin = (self.robot_pose.pose.pose.position.x,
self.robot_pose.pose.pose.position.y)
degrees = 90
self.plan_path(self.field_shape, origin, degrees)
rospy.loginfo("Path Planner: path planning complete.")
# Now wait for move_base
while not connected_to_move_base:
# Wait for the server for a while
connected_to_move_base = self.move_base_client.wait_for_server(dur)
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Update the user on the status of this process
msg = "Path Planner: waiting on move_base."
rospy.loginfo(msg)
# Now we are ready to start feeding move_base waypoints
return
def get_next_waypoint_index(self):
"""
Figures out what the index of the next waypoint is.
Iterates through the path_status's and finds the visiting one,
or the next not_visited one if not visiting on exists.
"""
for index, status in enumerate(self.path_status):
if status == 'visited':
continue
if status == 'visiting':
return index
if status == 'not_visited':
return index
# If I get here then there are no not_visited and we are done.
return None
def distance(self, p1, p2):
"""Returns the distance between two points."""
from math import sqrt
dx = p2.target_pose.pose.position.x - p1.target_pose.pose.position.x
dy = p2.target_pose.pose.position.y - p1.target_pose.pose.position.y
return sqrt(dx**2 + dy**2)
def step_path_following(self):
"""
Steps the path following system, checking if new waypoints
should be sent, if a timeout has occurred, or if path following
needs to be paused.
"""
# Visualize the data
self.visualize_path(self.path, self.path_status)
# Get the next (or current) waypoint
current_waypoint_index = self.get_next_waypoint_index()
# If the index is None, then we are done path planning
if current_waypoint_index == None:
rospy.loginfo("Path Planner: Done.")
return False
if current_waypoint_index == 0:
self.path_status[current_waypoint_index] = 'visited'
# Get the waypoint and status
current_waypoint = self.path[current_waypoint_index]
current_waypoint_status = self.path_status[current_waypoint_index]
# If the status is visited
print current_waypoint
if current_waypoint_status == 'visited':
# This shouldn't happen...
return True
# If the status is not_visited then we need to push the goal
if current_waypoint_status == 'not_visited':
# Cancel any current goals
#self.move_base_client.cancel_all_goals()
# Set it to visiting
self.path_status[current_waypoint_index] = 'visiting'
# Push the goal to the actionlib server
destination = MoveBaseGoal()
destination.target_pose.header.frame_id = self.field_frame_id
destination.target_pose.header.stamp = rospy.Time.now()
# Set the target location
destination.target_pose.pose.position.x = current_waypoint[0]
destination.target_pose.pose.position.y = current_waypoint[1]
# Calculate the distance
if self.previous_destination == None:
self.current_distance = 5.0
else:
self.current_distance = self.distance(self.previous_destination, destination)
# Set the heading
quat = qfe(0, 0, current_waypoint[2])
destination.target_pose.pose.orientation.x = quat[0]
destination.target_pose.pose.orientation.y = quat[1]
destination.target_pose.pose.orientation.z = quat[2]
destination.target_pose.pose.orientation.w = quat[3]
# Send the desired destination to the actionlib server
rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(current_waypoint))
self.current_destination = destination
self.move_base_client.send_goal(destination)
if self.current_distance < self.cut_spacing + 0.1:
rospy.sleep(0.1)
self.move_base_client.cancel_goal()
self.previous_destination = destination
# If the status is visiting, then we just need to monitor the status
temp_state = self.move_base_client.get_goal_status_text()
if current_waypoint_status == 'visiting':
if temp_state in ['ABORTED', 'SUCCEEDED']:
self.path_status[current_waypoint_index] = 'visited'
else:
duration = rospy.Duration(2.0)
from math import floor
count = 0
self.move_base_client.wait_for_result()
if self.timeout == True:
if count == 6+int(self.current_distance*4):
rospy.logwarn("Path Planner: move_base goal timeout occurred, clearing costmaps")
# Cancel the goals
self.move_base_client.cancel_all_goals()
# Clear the cost maps
self.clear_costmaps()
# Wait 1 second
rospy.Rate(1.0).sleep()
# Then reset the current goal
if not self.just_reset:
self.just_reset = True
self.path_status[current_waypoint_index] = 'not_visited'
else:
self.just_reset = False
self.path_status[current_waypoint_index] = 'visited'
return True
self.path_status[current_waypoint_index] = 'visited'
return True
class ArmCommander(Limb):
"""
This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages
Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation
"""
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {'fk': {'ros': 'compute_fk'},
'ik': {'ros': 'compute_ik',
'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name),
'trac': 'trac_ik_{}'.format(name)}}
self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK),
'func': self._get_fk_ros},
'kdl': {'func': self._get_fk_pykdl},
'robot': {'func': self._get_fk_robot}},
'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK),
'func': self._get_ik_ros},
'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK),
'func': self._get_ik_robot},
'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK),
'func': self._get_ik_trac},
'kdl': {'func': self._get_ik_pykdl}}}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name)
self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
######################################### CALLBACKS #########################################
def _cb_collision(self, msg):
if msg.collision_state:
with self._stop_lock:
self._stop_reason = 'collision'
def _cb_dig_io(self, msg):
if msg.state > 0:
with self._stop_lock:
self._stop_reason = 'cuff'
#############################################################################################
def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]]
def endpoint_name(self):
return self.name+'_gripper'
def group_name(self):
return self.name+'_arm'
def joint_limits(self):
xml_urdf = rospy.get_param('robot_description')
dict_urdf = xmltodict.parse(xml_urdf)
joints_urdf = []
joints_urdf.append([j['@name'] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()])
joints_urdf.append([[float(j['limit']['@lower']), float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()])
# reorder the joints limits
return dict(zip(self.joint_names(),
[joints_urdf[1][joints_urdf[0].index(name)] for name in self.joint_names()]))
def get_current_state(self, list_joint_names=[]):
"""
Returns the current RobotState describing all joint states
:param list_joint_names: If not empty, returns only the state of the requested joints
:return: a RobotState corresponding to the current state read on /robot/joint_states
"""
if len(list_joint_names) == 0:
list_joint_names = self.joint_names()
state = RobotState()
state.joint_state.name = list_joint_names
state.joint_state.position = map(self.joint_angle, list_joint_names)
state.joint_state.velocity = map(self.joint_velocity, list_joint_names)
state.joint_state.effort = map(self.joint_effort, list_joint_names)
return state
def get_ik(self, eef_poses, seeds=(), source=None, params=None):
"""
Return IK solutions of this arm's end effector according to the method declared in the constructor
:param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped
:param seeds: a single seed or a list of seeds of type RobotState for each input pose
:param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated)
:param params: dictionary containing optional non-generic IK parameters
:return: a list of RobotState for each input pose in input or a single RobotState
TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None)
Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose
"""
if not isinstance(eef_poses, list) or isinstance(eef_poses[0], list) and not isinstance(eef_poses[0][0], list):
eef_poses = [eef_poses]
if not seeds:
seeds=[]
elif not isinstance(seeds, list):
seeds = [seeds]*len(eef_poses)
input = []
for eef_pose in eef_poses:
if isinstance(eef_pose, list):
input.append(list_to_pose(eef_pose, self._world))
elif isinstance(eef_pose, PoseStamped):
input.append(eef_pose)
else:
raise TypeError("ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}".format(str(type(eef_pose))))
output = self._kinematics_services['ik'][self._selected_ik if source is None else source]['func'](input, seeds, params)
return output if len(eef_poses)>1 else output[0]
def get_fk(self, frame_id=None, robot_state=None):
"""
Return The FK solution oth this robot state according to the method declared in the constructor
robot_state = None will give the current endpoint pose in frame_id
:param robot_state: a RobotState message
:param frame_id: the frame you want the endpoint pose into
:return: [[x, y, z], [x, y, z, w]]
"""
if frame_id is None:
frame_id = self._world
if isinstance(robot_state, RobotState) or robot_state is None:
return self._kinematics_services['fk'][self._selected_fk]['func'](frame_id, robot_state)
else:
raise TypeError("ArmCommander.get_fk() accepts only a RobotState, got {}".format(str(type(robot_state))))
def _get_fk_pykdl(self, frame_id, state=None):
if state is None:
state = self.get_current_state()
fk = self._kinematics_pykdl.forward_position_kinematics(dict(zip(state.joint_state.name, state.joint_state.position)))
return [fk[:3], fk[-4:]]
def _get_fk_robot(self, frame_id = None, state=None):
# Keep this half-working FK, still used by generate_cartesian_path (trajectories.py)
if state is not None:
raise NotImplementedError("_get_fk_robot has no FK service provided by the robot except for its current endpoint pose")
ps = list_to_pose(self.endpoint_pose(), self._world)
return self._tf_listener.transformPose(frame_id, ps)
def _get_fk_ros(self, frame_id = None, state=None):
rqst = GetPositionFKRequest()
rqst.header.frame_id = self._world if frame_id is None else frame_id
rqst.fk_link_names = [self.endpoint_name()]
if isinstance(state, RobotState):
rqst.robot_state = state
elif isinstance(state, JointState):
rqst.robot_state.joint_state = state
elif state is None:
rqst.robot_state = self.get_current_state()
else:
raise AttributeError("Provided state is an invalid type")
fk_answer = self._kinematics_services['fk']['ros']['service'].call(rqst)
if fk_answer.error_code.val==1:
return fk_answer.pose_stamped[0]
else:
return None
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1],
[seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()] if len(seeds)>0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def _get_ik_robot(self, eef_poses, seeds=(), params=None):
ik_req = SolvePositionIKRequest()
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
ik_req.seed_mode = ik_req.SEED_USER if len(seeds) > 0 else ik_req.SEED_CURRENT
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['robot']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_trac(self, eef_poses, seeds=(), params=None):
ik_req = GetConstrainedPositionIKRequest()
if params is None:
ik_req.num_steps = 1
else:
ik_req.end_tolerance = params['end_tolerance']
ik_req.num_steps = params['num_attempts']
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
if len(seeds) == 0:
seeds = [self.get_current_state()]*len(eef_poses)
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['trac']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_ros(self, eef_poses, seeds=()):
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = self.group_name()
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
rqst.ik_request.pose_stamped = eef_pose # Do we really to do a separate call for each pose? _vector not used
ik_answer = self._kinematics_services['ik']['ros']['service'].call(rqst)
if len(seeds) > 0:
rqst.ik_request.robot_state = seeds[pose_num]
if ik_answer.error_code.val==1:
# Apply a filter to return only joints of this group
try:
ik_answer.solution.joint_state.velocity = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.velocity) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()]
ik_answer.solution.joint_state.effort = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.effort) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()]
except IndexError:
pass
ik_answer.solution.joint_state.position = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.position) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()]
ik_answer.solution.joint_state.name = [joint for joint in ik_answer.solution.joint_state.name if joint in self.joint_names()]
solutions.append(ik_answer.solution)
else:
solutions.append(None)
return solutions
def translate_to_cartesian(self, path, frame_id, time, n_points=50, max_speed=np.pi/4, min_success_rate=0.5, display_only=False,
timeout=0, stop_test=lambda:False, pause_test=lambda:False):
"""
Translate the end effector in straight line, following path=[translate_x, translate_y, translate_z] wrt frame_id
:param path: Path [x, y, z] to follow wrt frame_id
:param frame_id: frame_id of the given input path
:param time: Time of the generated trajectory
:param n_points: Number of 3D points of the cartesian trajectory
:param max_speed: Maximum speed for every single joint in rad.s-1, allowing to avoid jumps in joints configuration
:param min_success_rate: Raise RuntimeError in case the success rate is lower than min_success_rate
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up (default is 0 = do not retry)
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return:
:raises: RuntimeError if success rate is too low
"""
def trajectory_callable(start):
traj, success_rate = trajectories.generate_cartesian_path(path, frame_id, time, self, None, n_points, max_speed)
if success_rate < min_success_rate:
raise RuntimeError("Unable to generate cartesian path (success rate : {})".format(success_rate))
return traj
return self._relaunched_move_to(trajectory_callable, display_only, timeout, stop_test, pause_test)
def move_to_controlled(self, goal, rpy=[0, 0, 0], display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False):
"""
Move to a goal using interpolation in joint space with limitation of velocity and acceleration
:param goal: Pose, PoseStamped or RobotState
:param rpy: Vector [Roll, Pitch, Yaw] filled with 0 if this axis must be preserved, 1 otherwise
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return: None
:raises: ValueError if IK has no solution
"""
assert callable(stop_test)
assert callable(pause_test)
if not isinstance(goal, RobotState):
goal = self.get_ik(goal)
if goal is None:
raise ValueError('This goal is not reachable')
# collect the robot state
start = self.get_current_state()
# correct the orientation if rpy is set
if np.array(rpy).any():
# convert the starting point to rpy pose
pos, rot = states.state_to_pose(start,
self,
True)
for i in range(3):
if rpy[i]:
rpy[i] = rot[i]
goal = states.correct_state_orientation(goal, rpy, self)
# parameters for trapezoidal method
kv_max = self.kv_max
ka_max = self.ka_max
return self._relaunched_move_to(lambda start: trajectories.trapezoidal_speed_trajectory(goal, start=start ,kv_max=kv_max, ka_max=ka_max),
display_only, timeout, stop_test, pause_test)
def rotate_joint(self, joint_name, goal_angle, display_only=False, stop_test=lambda:False, pause_test=lambda:False):
goal = self.get_current_state()
joint = goal.joint_state.name.index(joint_name)
# JTAS accepts all angles even out of limits
#limits = self.joint_limits()[joint_name]
goal.joint_state.position[joint] = goal_angle
return self.move_to_controlled(goal, display_only=display_only, stop_test=stop_test, pause_test=pause_test)
def _relaunched_move_to(self, trajectory_callable, display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False):
"""
Relaunch several times (until cuff interaction or failure) a move_to() whose trajectory is generated by the callable passed in parameter
:param trajectory_callable: Callable to call to recompute the trajectory
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
:return:
"""
assert callable(trajectory_callable)
retry = True
t0 = rospy.get_time()
while retry and rospy.get_time()-t0 < timeout or timeout == 0:
start = self.get_current_state()
trajectory = trajectory_callable(start)
if display_only:
self.display(trajectory)
break
else:
retry = not self.execute(trajectory, test=lambda: stop_test() or pause_test())
if pause_test():
if not stop_test():
retry = True
while pause_test():
rospy.sleep(0.1)
if timeout == 0:
return not display_only and not retry
if retry:
rospy.sleep(1)
return not display_only and not retry
def get_random_pose(self):
# get joint names
joint_names = self.joint_names()
# create a random joint state
bounds = []
for key, value in self.joint_limits().iteritems():
bounds.append(value)
bounds = np.array(bounds)
joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1], len(joint_names))
# append it in a robot state
goal = RobotState()
goal.joint_state.name = joint_names
goal.joint_state.position = joint_state
goal.joint_state.header.stamp = rospy.Time.now()
goal.joint_state.header.frame_id = 'base'
return goal
######################## OPERATIONS ON TRAJECTORIES
# TO BE MOVED IN trajectories.py
def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time*(1.0/nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1)
joints.append(pose_lin[1:].tolist())
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i+1)*dt)
# append the position to the message
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory))))
self._display_traj.publish(dt)
def execute(self, trajectory, test=None, epsilon=0.1):
"""
Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
Non-blocking needs should deal with the JointTrajectory action server
:param trajectory: either a RobotTrajectory or a JointTrajectory
:param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
:return: True if execution ended successfully, False otherwise
"""
def distance_to_first_point(point):
joint_pos = np.array(point.positions)
return np.linalg.norm(current_array - joint_pos)
self.display(trajectory)
with self._stop_lock:
self._stop_reason = ''
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
elif not isinstance(trajectory, JointTrajectory):
raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
ftg = FollowJointTrajectoryGoal()
ftg.trajectory = trajectory
# check if it is necessary to move to the first point
current = self.get_current_state()
current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])
if distance_to_first_point(trajectory.points[0]) > epsilon:
# convert first point to robot state
rs = RobotState()
rs.joint_state.name = trajectory.joint_names
rs.joint_state.position = trajectory.points[0].positions
# move to the first point
self.move_to_controlled(rs)
# execute the input trajectory
self.client.send_goal(ftg)
# Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory
while self.client.simple_state != SimpleGoalState.DONE:
if callable(test) and test():
self.client.cancel_goal()
return True
if self._stop_reason!='':
self.client.cancel_goal()
return False
self._rate.sleep()
return True
def close(self):
"""
Open the gripper
:return: True if an object has been grasped
"""
return self._gripper.close(True)
def open(self):
"""
Close the gripper
return: True if an object has been released
"""
return self._gripper.open(True)
def gripping(self):
return self._gripper.gripping()
def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True):
"""
Blocks until external motion is given to the arm
:param threshold:
:param rate: rate of control loop in Hertz
:param ignore_gripping: True if we must wait even if no object is gripped
"""
def detect_variation():
new_effort = np.array(self.get_current_state([self.name+'_w0',
self.name+'_w1',
self.name+'_w2']).joint_state.effort)
delta = np.absolute(effort - new_effort)
return np.amax(delta) > threshold
# record the effort at calling time
effort = np.array(self.get_current_state([self.name+'_w0',
self.name+'_w1',
self.name+'_w2']).joint_state.effort)
# loop till the detection of changing effort
rate = rospy.Rate(rate)
while not detect_variation() and not rospy.is_shutdown() and (ignore_gripping or self.gripping()):
rate.sleep()
goal = GenericExecuteGoal()
if len(sys.argv) > 1:
location = str(sys.argv[1]).upper()
else:
location = "PP01"
goal.parameters.append(KeyValue(key="location", value=location))
rospy.loginfo("Sending following goal to perceive cavity server")
rospy.loginfo(goal)
client.send_goal(goal)
timeout = 15.0
finished_within_time = client.wait_for_result(
rospy.Duration.from_sec(int(timeout)))
if not finished_within_time:
client.cancel_goal()
state = client.get_state()
result = client.get_result()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Action SUCCESS")
for kv in result.results:
rospy.loginfo(kv.value)
elif state == GoalStatus.ABORTED:
rospy.logerr("Action FAILED")
else:
rospy.logwarn("State: " + str(state))
rospy.loginfo(result)
class Strategizer():
def __init__(self, waypoints_file):
rospy.loginfo("Initializing Strategizer")
self.cone_coord = None
self.cone_capture_mode = False
self.transformListener = tf.TransformListener()
self.load_waypoints()
self.setup_goal_actions()
self.wait_for_odom()
self.send_first_goal()
def load_waypoints(self):
waypoint_file_reader = WaypointFileReader()
self.waypoints = waypoint_file_reader.read_file(waypoints_file)
if len(self.waypoints) <= 0:
raise ROSInitException("No waypoints given!")
rospy.loginfo("Strategizer loaded with waypoints:\n %s" % self.waypoints)
def setup_goal_actions(self):
rospy.logwarn("Waiting for required services...")
self.capture_cone_client = SimpleActionClient('capture_cone', CaptureWaypointAction)
self.capture_cone_client.wait_for_server()
rospy.loginfo("capture_cone action client loaded")
self.capture_waypoint_client = SimpleActionClient('capture_waypoint', CaptureWaypointAction)
self.capture_waypoint_client.wait_for_server()
rospy.loginfo("capture_waypoint action client loaded")
rospy.wait_for_service('mux_cmd_vel/select')
rospy.loginfo("mux_cmd_vel client loaded")
rospy.logwarn("Services loaded")
def wait_for_odom(self):
self.odom = None
rospy.Subscriber('odom', Odometry, self.setup_odom_callback())
while self.odom is None:
rospy.logwarn("Waiting for initial odometry...")
rospy.logwarn("Initial odom loaded! Sleeping for 3 sec...")
rospy.sleep(3)
def setup_odom_callback(self):
def odom_callback(data):
self.odom = data
return odom_callback
def send_first_goal(self):
rospy.loginfo("Strategizer sending initial goal")
self.current_waypoint_idx = 0
self.send_next_capture_waypoint_goal()
# monitor the cone_coord topic
def setup_cone_coord_callback(self):
def cone_coord_callback(data):
self.cone_coord = data
return cone_coord_callback
# check if next waypoint is a cone, and we are sufficiently close
# to switch to cone tracking mode
def check_for_cone(self):
if self.current_waypoint_idx == len(self.waypoints):
rospy.logwarn("All waypoints reached! Finished strategizing.")
return
self.flush_outdated_cone_coord_data()
# if we're already in cone-capture mode, or
# the current waypoint is not a cone... continue
if self.cone_capture_mode or \
self.waypoints[self.current_waypoint_idx].type != 'C':
return
# if we are sufficiently close, switch to cone capture mode
cone_waypoint = self.waypoints[self.current_waypoint_idx]
cone_coord_in_base_link_frame = self.waypoint_in_base_link_frame(cone_waypoint)
(x,y) = cone_coord_in_base_link_frame.point.x, cone_coord_in_base_link_frame.point.y
distance_to_cone = math.sqrt(x*x + y*y)
if (distance_to_cone < settings.DISTANCE_TO_CAPTURE):
if self.cone_coord:
self.switch_to_cone_capture_mode()
else:
rospy.logwarn("Should be close enough to the cone, but no visual yet! Moving closer...")
if (self.cone_coord is None and distance_to_cone < settings.DISTANCE_TO_CAPTURE_NO_VISUAL):
rospy.logwarn("We're within %f m of the cone, and still have no visual. Switching to cone capture mode anyway." % settings.DISTANCE_TO_CAPTURE_NO_VISUAL)
self.switch_to_cone_capture_mode()
def waypoint_in_base_link_frame(self, waypoint_msg):
waypoint = PointStamped()
waypoint.header.frame_id = "/map"
waypoint.header.stamp = self.transformListener.getLatestCommonTime("/base_link", "/map")
waypoint.point = waypoint_msg.coordinate
return self.transformListener.transformPoint("/base_link", waypoint)
def flush_outdated_cone_coord_data(self):
if self.cone_coord is None:
return
now = rospy.Time.now().to_sec()
latency = now - self.cone_coord.header.stamp.to_sec()
if latency > 0.5:
self.cone_coord = None
def send_next_capture_waypoint_goal(self):
waypoint_to_capture = self.waypoints[self.current_waypoint_idx]
rospy.logwarn("Moving towards next goal:\n %s" % waypoint_to_capture)
goal = CaptureWaypointGoal()
goal.waypoint = waypoint_to_capture
self.capture_waypoint_client.send_goal(goal, self.recieve_capture_waypoint_done_callback)
def recieve_capture_waypoint_done_callback(self, status, result):
rospy.logwarn("capture_waypoint returned with status: %s" % status)
if status == GoalStatus.PREEMPTED:
# we cancelled the goal, so capture_cone should be taking care of things... do nothing
return
if status != GoalStatus.SUCCEEDED:
rospy.logerr("capture_waypoint failed to achieve goal! result = %s" % result)
rospy.logwarn("skipping this waypoint...")
self.current_waypoint_idx = self.current_waypoint_idx + 1
if self.current_waypoint_idx == len(self.waypoints):
rospy.logwarn("All waypoints reached! Finished strategizing.")
else:
self.send_next_capture_waypoint_goal()
def switch_to_cone_capture_mode(self):
rospy.logwarn("Switching to cone capture mode!")
# cancel the capture_waypoint goal
self.capture_waypoint_client.cancel_goal()
rospy.loginfo("capture_waypoint goal cancelled")
# switch to cone_capture node for publishing to /cmd_vel
self.cone_capture_mode = True
self.switch_cmd_vel("cmd_vel_capture_cone")
# send the goal to capture_cone node
waypoint_to_capture = self.waypoints[self.current_waypoint_idx]
rospy.logwarn("Attempting to capture cone at:\n %s" % waypoint_to_capture)
goal = CaptureWaypointGoal()
goal.waypoint = waypoint_to_capture
self.capture_cone_client.send_goal(goal, self.receive_capture_cone_done_callback)
def receive_capture_cone_done_callback(self, status, result):
rospy.logwarn("capture_cone returned with status: %s" % status)
if status != GoalStatus.SUCCEEDED:
rospy.logerr("capture_cone failed to achieve goal! result = %s" % result)
rospy.logwarn("skipping this waypoint...")
self.current_waypoint_idx = self.current_waypoint_idx + 1
if self.current_waypoint_idx == len(self.waypoints):
rospy.logwarn("All waypoints reached! Finished strategizing.")
else:
# switch back to capture_waypoint for next waypoint
self.switch_cmd_vel("cmd_vel_capture_waypoint")
self.cone_capture_mode = False
self.send_next_capture_waypoint_goal()
def switch_cmd_vel(self, topic):
rospy.loginfo("switching cmd_vel_mux to %s" % topic)
cmd_vel_mux_select = rospy.ServiceProxy('mux_cmd_vel/select', MuxSelect)
resp = cmd_vel_mux_select(topic)
rospy.loginfo("result of switching cmd_vel_mux to %s: %s" % (topic, resp))
def publish_waypoint_markers(self):
pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray)
marker_array = MarkerArray()
for i in range(len(self.waypoints)):
waypoint = self.waypoints[i]
waypoint_marker = Marker()
waypoint_marker.id = i
waypoint_marker.header.frame_id = "/map"
waypoint_marker.header.stamp = rospy.Time.now()
if (waypoint.type == 'P'):
waypoint_marker.type = 5 # Line List
waypoint_marker.text = 'waypoint_%s_point' % i
waypoint_marker.color.r = 176.0
waypoint_marker.color.g = 224.0
waypoint_marker.color.b = 230.0
waypoint_marker.color.a = 0.5
waypoint_marker.scale.x = 0.5
c = waypoint.coordinate
waypoint_marker.points.append(Point(c.x, c.y, c.z))
waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0))
else:
waypoint_marker.type = 3 # Cylinder
waypoint_marker.text = 'waypoint_%s_cone' % i
waypoint_marker.color.r = 255.0
waypoint_marker.color.g = 69.0
waypoint_marker.color.b = 0.0
waypoint_marker.color.a = 1.0
waypoint_marker.scale.x = 0.3
waypoint_marker.scale.y = 0.3
waypoint_marker.scale.z = 0.5
waypoint_marker.pose.position = waypoint.coordinate
marker_array.markers.append(waypoint_marker)
if self.current_waypoint_idx != len(self.waypoints):
current_waypoint_marker = Marker()
current_waypoint_marker.id = 999
current_waypoint_marker.header.frame_id = "/map"
current_waypoint_marker.header.stamp = rospy.Time.now()
current_waypoint_marker.type = 2 # Sphere
current_waypoint_marker.text = 'current_waypoint'
current_waypoint_marker.color.r = 255.0
current_waypoint_marker.color.g = 0.0
current_waypoint_marker.color.b = 0.0
current_waypoint_marker.color.a = 1.0
current_waypoint_marker.scale.x = 0.3
current_waypoint_marker.scale.y = 0.3
current_waypoint_marker.scale.z = 0.3
current_waypoint = self.waypoints[self.current_waypoint_idx]
current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x
current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y
current_waypoint_marker.pose.position.z = 1.0
marker_array.markers.append(current_waypoint_marker)
pub_waypoint_markers.publish(marker_array)
