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)