class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while(not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
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 HeadClient(): def __init__(self): name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() self.hor_pos = 0.0 self.vert_pos = 0.0 def move_head_vert (self, vert_val): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'torso_lift_link' head_goal.max_velocity = .3 self.vert_pos = vert_val/50.0 - 1 head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos) self.head_client.send_goal(head_goal) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') def move_head_hor (self, hor_val): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'torso_lift_link' head_goal.max_velocity = .3 self.hor_pos = -(hor_val/20.0 - 2.5) head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos) self.head_client.send_goal(head_goal) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class GripperClient(): def __init__(self): #rospy.init_node('simplegui_gripper_node', anonymous=True) name_space_r = '/r_gripper_controller/gripper_action' self.r_gripper_client = SimpleActionClient(name_space_r, GripperCommandAction) name_space_l = '/l_gripper_controller/gripper_action' self.l_gripper_client = SimpleActionClient(name_space_l, GripperCommandAction) self.r_gripper_client.wait_for_server() self.l_gripper_client.wait_for_server() def command(self, left, close): gripper_client = self.l_gripper_client if not left: gripper_client = self.r_gripper_client gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 30.0 if close: gripper_goal.command.position = 0.0 else: gripper_goal.command.position = 1.0 gripper_client.send_goal(gripper_goal)
class ActionTasks: def __init__(self, script_path): rospy.init_node('action_testr') rospy.on_shutdown(self.cleanup) self.fridge = (Pose(Point(0.295, -2.304, 0.0), Quaternion(0.0, 0.0, -0.715, 0.699))) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() self.move_to(self.fridge) def move_to(self, location): goal = MoveBaseGoal() goal.target_pose.pose = location goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) #self.client.wait_for_result() self.client.wait_for_result(rospy.Duration.from_sec(40.0)) if self.client.get_state() == GoalStatus.SUCCEEDED: result = self.client.get_result() print "Result: SUCCEEDED " elif self.client.get_state() == GoalStatus.PREEMPTED: print "Action pre-empted" else: print "Action failed" def cleanup(self): rospy.loginfo("Shutting down talk node...")
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
class DoReset(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['reset_plan']) self.eef_pub = rospy.Publisher('/CModelRobotOutput', eef_cmd, queue_size=1, latch=True) self.move_arm_client = SimpleActionClient('/wm_arm_driver_node/execute_plan', executePlanAction) def execute(self, ud): rospy.logdebug("Entered 'RO_RESET' state.") hand_cmd = eef_cmd() hand_cmd.rACT = 1 # activate gripper hand_cmd.rGTO = 1 # request to go to position hand_cmd.rSP = 200 # set activation speed (0[slowest]-255[fastest]) hand_cmd.rFR = 0 # set force limit (0[min] - 255[max]) hand_cmd.rPR = 0 # request to open self.eef_pub.publish(hand_cmd) self.move_arm_client.wait_for_server() goal = executePlanGoal() goal.trajectory = ud.reset_plan self.move_arm_client.send_goal(goal) self.move_arm_client.wait_for_result() status = self.move_arm_client.get_state() if status == GoalStatus.SUCCEEDED: return 'succeeded' elif status == GoalStatus.PREEMPTED: return 'preempted' else: return 'aborted'
def lookAtLocation(userdata): head_goal = PointHeadGoal() head_goal.target.header.frame_id = "base_link" # Iterate until we find the object we are searching for index_of_object = 0 while index_of_object < 10: if userdata.object_found.object_list[index_of_object].name == userdata.object_to_search_for: break; index_of_object += 1 head_goal.target.point.x = userdata.object_found.object_list[index_of_object].pose.position.x head_goal.target.point.y = userdata.object_found.object_list[index_of_object].pose.position.y head_goal.target.point.z = userdata.object_found.object_list[index_of_object].pose.position.z head_goal.pointing_frame = "stereo_link" head_goal.pointing_axis.x = 1.0 head_goal.pointing_axis.y = 0.0 head_goal.pointing_axis.z = 0.0 head_goal.max_velocity = 1.5 head_goal.min_duration.secs = 1.5 client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction) client.wait_for_server(rospy.Duration(0.5)) client.send_goal(head_goal) return succeeded
class DropService(): def __init__(self): rospy.init_node(NAME + 'server' , anonymous=True) self.client = SimpleActionClient("smart_arm_action", SmartArmAction) self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction) self.client.wait_for_server() self.gripper_client.wait_for_server() self.service = rospy.Service('drop_service', ArmDropService, self.handle_drop) rospy.loginfo("%s: Ready for Dropping", NAME) #drop it like it's hot def handle_drop(self, req): rospy.loginfo("told to drop") success = False #this seems to be as far as it can open (both straight ahead), same vals anh uses if self.move_gripper(0.2, -0.2): success = True return success #stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks def move_gripper(self, left_finger, right_finger): goal = SmartArmGripperGoal() goal.target_joints = [left_finger, right_finger] self.gripper_client.send_goal(goal) self.gripper_client.wait_for_goal_to_finish() result = self.gripper_client.get_result() if result.success == False: rospy.loginfo("Drop failed") else: rospy.loginfo("Object Released! Hopefully it isn't sticky")
class Move(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['wp_reached', 'wp_not_reached', 'test_finished'], input_keys=['move_waypoints', 'move_target_wp', 'move_wp_str'], output_keys=['move_target_wp']) self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True) def execute(self, ud): self.move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose = ud.move_waypoints[ud.move_target_wp] goal.target_pose.header.stamp = rospy.Time.now() self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result() status = self.move_base_client.get_state() tts_msg = String() if status == GoalStatus.SUCCEEDED: tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "." self.tts_pub.publish(tts_msg) if ud.move_target_wp == 0: return 'test_finished' else: ud.move_target_wp -= 1 return 'succeeded' else: return 'wp_not_reached'
def init_jt_client(arm = 'r'): name = "".join( [ arm, "_arm_controller/joint_trajectory_action" ] ) client = SimpleActionClient(name, JointTrajectoryAction) rospy.loginfo( 'waiting for action client : %s ...'%name ) client.wait_for_server() rospy.loginfo( '%s is up and running!'%name ) return client
class NavServer(object): def __init__(self, name): rospy.loginfo("Starting '%s'." % name) self._ps = PNPSimplePluginServer( name=name, ActionSpec=StringAction, execute_cb=self.execute_cb, auto_start=False ) self._ps.register_preempt_callback(self.preempt_cb) self.client = SimpleActionClient('topological_navigation', GotoNodeAction) self.client.wait_for_server() self._ps.start() rospy.loginfo("Done") def execute_cb(self, goal): self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value)) result = self.client.get_state() if result == GoalStatus.PREEMPTED: return if result == GoalStatus.SUCCEEDED: self._ps.set_succeeded() else: self._ps.set_aborted() def preempt_cb(self): self.client.cancel_all_goals() self._ps.set_preempted()
class Gripper: LEFT = "l" RIGHT = "r" def __init__(self, direction): assert direction == Gripper.LEFT or direction == Gripper.RIGHT self.direction = direction name_space = "/{0}_gripper_controller/gripper_action".format(self.direction) self.gripper_client = SimpleActionClient(name_space, GripperCommandAction) self.gripper_client.wait_for_server() def open_gripper(self, wait=False): self.change_state(0.08, wait) def close_gripper(self, wait=False): self.change_state(0.0, wait) def change_state(self, value, wait): gripper_goal = GripperCommandGoal() gripper_goal.command.position = value gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal) if wait: self.gripper_client.wait_for_result() if not self.gripper_client.get_result().reached_goal: time.sleep(1)
def main(): rospy.init_node('trigger_open_door') # initial door prior_door = Door() prior_door.frame_p1.x = 1.0 prior_door.frame_p1.y = -0.5 prior_door.frame_p2.x = 1.0 prior_door.frame_p2.y = 0.5 prior_door.door_p1.x = 1.0 prior_door.door_p1.y = -0.5 prior_door.door_p2.x = 1.0 prior_door.door_p2.y = 0.5 prior_door.travel_dir.x = 1.0 prior_door.travel_dir.y = 0.0 prior_door.travel_dir.z = 0.0 prior_door.rot_dir = Door.ROT_DIR_COUNTERCLOCKWISE prior_door.hinge = Door.HINGE_P2 prior_door.header.frame_id = "base_footprint" door = DoorGoal() door.door = prior_door ac = SimpleActionClient('move_through_door', DoorAction) print 'Waiting for open door action server' ac.wait_for_server() print 'Sending goal to open door action server' ac.send_goal_and_wait(door, rospy.Duration(500.0), rospy.Duration(2.0))
class Hello: """ a class to greet people need to recognise people """ def __init__(self): self.voice = rospy.get_param("~voice", "voice_en1_mbrola") # Create the sound client object self.soundhandle = SoundClient() # Wait a moment to let the client connect to the # sound_play server rospy.sleep(1) # Make sure any lingering sound_play processes are stopped. self.soundhandle.stopAll() #set action server self.client = SimpleActionClient('face_recognition', face_recognition.msg.FaceRecognitionAction) # listening for goals. self.client.wait_for_server() rospy.sleep(2) #result will be published on this topic rospy.Subscriber("/face_recognition/result", FaceRecognitionActionFeedback, self.face_found) rospy.Subscriber("/face_recognition/feedback", FaceRecognitionActionFeedback, self.Unknown) self.look_for_face() def look_for_face(self): ''' send command look for face once ''' goal = face_recognition.msg.FaceRecognitionGoal(order_id=0, order_argument="none") self.client.send_goal(goal) #self.client.wait_for_result(rospy.Duration.from_sec(6.0)) #if self.client.get_state() == GoalStatus.FAILED: #self.unknown() def face_found(self,msg): ''' clean up feedback to extract the name check if greeted before answer accordingly ''' person = str(msg.result.names[0]) rospy.logwarn(person) self.soundhandle.say("hello "+ person ,self.voice) #self.task1 = "hello" + person #rospy.sleep(2) #return the processed value self.value = str(self.task1) def Unknown(self,msg): self.soundhandle.say("hello what is your name " ,self.voice) def __str__(self): return
class Arm: def __init__(self, arm): if arm == 'r': self.arm_client = SAC('r_arm_controller/joint_trajectory_action', JointTrajectoryAction) self.arm = arm elif arm == 'l': self.arm_client = SAC('l_arm_controller/joint_trajectory_action', JointTrajectoryAction) self.arm = arm self.arm_client.wait_for_server() rospy.loginfo('Waiting for server...') def move(self, angles, time, blocking): angles = [angles] times = [time] timeout=times[-1] + 1.0 goal = JointTrajectoryGoal() goal.trajectory.joint_names =self.get_joint_names(self.arm) for (ang, t) in zip(angles, times): point = JointTrajectoryPoint() point.positions = ang point.time_from_start = rospy.Duration(t) goal.trajectory.points.append(point) goal.trajectory.header.stamp = rospy.Time.now() self.arm_client.send_goal(goal) rospy.sleep(.1) rospy.loginfo("command sent to client") status = 0 if blocking: #XXX why isn't this perfect? end_time = rospy.Time.now() + rospy.Duration(timeout+ .1) while ( (not rospy.is_shutdown()) and\ (rospy.Time.now() < end_time) and\ (status < gs.SUCCEEDED) and\ (type(self.arm_client.action_client.last_status_msg) != type(None))): status = self.arm_client.action_client.last_status_msg.status_list[-1].status #XXX get to 80 rospy.Rate(10).sleep() if status >gs.SUCCEEDED: rospy.loginfo("goal status achieved. exiting") else: rospy.loginfo("ending due to timeout") result = self.arm_client.get_result() return result def get_joint_names(self, char): return [char+'_shoulder_pan_joint', char+'_shoulder_lift_joint', char+'_upper_arm_roll_joint', char+'_elbow_flex_joint', char+'_forearm_roll_joint', char+'_wrist_flex_joint', char+'_wrist_roll_joint' ]
def close_gripper(which_arm): """ close the gripper """ gripper_client = SimpleActionClient(which_arm[0] + '_gripper_controller/gripper_action', Pr2GripperCommandAction) gripper_client.wait_for_server() ##### Define goals for gripper: close all the way goal_close = Pr2GripperCommandGoal() goal_close.command.position = 0.0 goal_close.command.max_effort = 20.0 gripper_client.send_goal_and_wait(goal_close) # open gripper for knob
def open_gripper(which_arm): """ Open the gripper """ gripper_client = SimpleActionClient(which_arm[0] + '_gripper_controller/gripper_action', Pr2GripperCommandAction) gripper_client.wait_for_server() ##### Define goals for gripper: open all the way goal_open = Pr2GripperCommandGoal() goal_open.command.position = 0.05 goal_open.command.max_effort = 20.0 gripper_client.send_goal_and_wait(goal_open) # open gripper for switch-flipping
class RecordPoseStampedFromPlayMotion(): """Manage the learning from a play motion gesture""" def __init__(self): rospy.loginfo("Initializing RecordFromPlayMotion") rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'") self.play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction) self.play_motion_as.wait_for_server() rospy.loginfo("Connected.") self.current_rosbag_name = "uninitialized_rosbag_name" self.lfee = LearnFromEndEffector(['/tf_to_ps'], ['right_arm']) def play_and_record(self, motion_name, groups=['right_arm'], bag_name="no_bag_name_set"): """Play the specified motion and start recording poses. Try to get the joints to record from the metadata of the play_motion gesture or, optionally, specify the joints to track""" # Check if motion exists in param server PLAYMOTIONPATH = '/play_motion/motions/' if not rospy.has_param(PLAYMOTIONPATH + motion_name): rospy.logerr("Motion named: " + motion_name + " does not exist in param server at " + PLAYMOTIONPATH + motion_name) return else: rospy.loginfo("Found motion " + motion_name + " in param server at " + PLAYMOTIONPATH + motion_name) # Get it's info motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name) # check if joints was specified, if not, get the joints to actually save if len(groups) > 0: joints_to_record = groups else: joints_to_record = motion_info['groups'] rospy.loginfo("Got groups: " + str(joints_to_record)) # play motion rospy.loginfo("Playing motion!") pm_goal = PlayMotionGoal(motion_name, False, 0) self.play_motion_as.send_goal(pm_goal) self.lfee.start_learn(motion_name, bag_name) done_with_motion = False while not done_with_motion: state = self.play_motion_as.get_state() #rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state]) if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED: done_with_motion = True elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE: rospy.logerr("We got state " + str(state) + " unexpectedly, motion failed. Aborting.") return None rospy.sleep(0.1) # data is written to rosbag in here motion_data = self.lfee.stop_learn() return motion_data
def look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class HeadNode(): def __init__(self): name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() def moveHead(self, theta, phi): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(math.cos(theta), math.sin(theta), phi) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration(10.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class HeadObjectTracking(): def __init__(self): name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() self.curr_tracking_point = Point(1, 0, 0.5) self.point_head(self.curr_tracking_point.x, self.curr_tracking_point.y, self.curr_tracking_point.z) rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.new_tracking_data) def new_tracking_data(self, marker): """ Adds a new tracking data point for the head. Points the head to a point taken as a moving average over some number of previous tracking data points. """ pos = marker.pose.pose.position OLD_DATA_WEIGHT = .3 # calculate the moving average of the x, y, z positions tracking_point = self.curr_tracking_point avg_x = (self.curr_tracking_point.x * OLD_DATA_WEIGHT) + (pos.x * (1 - OLD_DATA_WEIGHT)) avg_y = (self.curr_tracking_point.y * OLD_DATA_WEIGHT) + (pos.y * (1 - OLD_DATA_WEIGHT)) avg_z = (self.curr_tracking_point.z * OLD_DATA_WEIGHT) + (pos.z * (1 - OLD_DATA_WEIGHT)) # make a new averaged point to track and point the head there self.curr_tracking_point = Point(avg_x, avg_y, avg_z) self.point_head(avg_x, avg_y, avg_z) def point_head(self, x, y, z): """ Point the head to the specified point """ head_goal = PointHeadGoal() head_goal.target.header.frame_id = '/torso_lift_link' head_goal.max_velocity = .3 # The transform seems to aim high. Move it down a little... head_goal.target.point = Point(x, y, z - .4) rospy.logdebug('Moving head to\n' + str(head_goal.target.point)) self.head_client.send_goal(head_goal)
def __init__(self): rospy.init_node('pr2_pose_morph') self.joint_map = {} self.joint_map[LEFTY] = ['l_%s'%a for a in ARM_JOINTS] self.joint_map[RIGHTY] = ['r_%s'%a for a in ARM_JOINTS] self.joint_map[HEAD] = ['head_pan_joint', 'head_tilt_joint'] self.joint_servers = {} for action_name in self.joint_map: server = SimpleActionClient(action_name, JointTrajectoryAction) self.joint_servers[action_name] = server self.nav_server = SimpleActionClient("/base_controller/move_sequence", MoveSequenceAction) for server in [self.nav_server] + self.joint_servers.values(): rospy.loginfo("Waiting for %s..."%server.action_client.ns) server.wait_for_server() rospy.loginfo("Ready")
class Torso: def __init__(self): name_space = '/torso_controller/position_joint_action' self.torso_client = SimpleActionClient(name_space, SingleJointPositionAction) self.torso_client.wait_for_server() def move(self, position): assert(position >= 0.0 and position <= 0.2) self.position = position up = SingleJointPositionGoal() up.position = self.position up.min_duration = rospy.Duration(2.0) up.max_velocity = 1.0 self.torso_client.send_goal(up) self.torso_client.wait_for_result()
def move_head(): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = self.get_frame() head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = self.get_target() head_goal.max_velocity = 10.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(2.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') self.gui.show_text_in_rviz("Head!")
class Move_Head: """ a class to fins and pick up a object """ def __init__(self, text): #self.task = text#whole message from greeting self.head_client = SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.head_client.wait_for_server() ar = text.split(':') cmd = ar[0] direction = ar[5] if direction =='left': self.move_head(1.1, 0.0) if direction =='right': self.move_head(-1.1, 0.0) if direction =='front': self.move_head(0.0, 0.0) def move_head(self, pan , tilt): # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_mod_joint'] # Create a single-point head trajectory with the head_goal as the end-point head_trajectory = JointTrajectory() head_trajectory.joint_names = head_joints head_trajectory.points.append(JointTrajectoryPoint()) head_trajectory.points[0].positions = pan , tilt head_trajectory.points[0].velocities = [0.0 for i in head_joints] head_trajectory.points[0].accelerations = [0.0 for i in head_joints] head_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = head_trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal self.head_client.send_goal(head_goal) # Wait for up to 5 seconds for the motion to complete self.head_client.wait_for_result(rospy.Duration(2.0))
def fixHeadPosition(userdata): head_goal = PointHeadGoal() head_goal.target.header.frame_id = "base_link" head_goal.target.point.x = 1.0 head_goal.target.point.y = 0.0 head_goal.target.point.z = 1.65 head_goal.pointing_frame = "stereo_link" head_goal.pointing_axis.x = 1.0 head_goal.pointing_axis.y = 0.0 head_goal.pointing_axis.z = 0.0 head_goal.max_velocity = 1.5 head_goal.min_duration.secs = 1.5 client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction) client.wait_for_server(rospy.Duration(5.0)) client.send_goal(head_goal) return succeeded
def tilt_head(self, down=True): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = self.get_frame() head_goal.min_duration = rospy.Duration(0.3) if down: head_goal.target.point = Point(1, 0, Head.speed * -0.1) else: head_goal.target.point = Point(1, 0, Head.speed * 0.1) head_goal.max_velocity = 10.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(2.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
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 main(): rospy.init_node("simple_navigation_goals") move_base_client = SimpleActionClient('move_base', MoveBaseAction) rospy.loginfo('Connecting to server') move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'komodo_1/base_link' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = -1.0 goal.target_pose.pose.orientation.w = 1.0 rospy.loginfo('Sending goal') move_base_client.send_goal(goal) move_base_client.wait_for_result() if move_base_client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Hooray, the base moved 1 meter forward') else: rospy.loginfo('The base failed to move forward 1 meter for some reason')
class AUVController: """Class for controlling AUV in missions. The class is actually a unified wrap over actions and services calls. """ # TODO: Errors handling # TODO: Devices support def __init__(self): """The constructor. """ self.rotation_client_ = SimpleActionClient('stingray_action_rotate', stingray_movement_msgs.msg.RotateAction) self.dive_client_ = SimpleActionClient('stingray_action_dive', stingray_movement_msgs.msg.DiveAction) self.linear_client_ = SimpleActionClient('stingray_action_linear_movement', stingray_movement_msgs.msg.LinearMoveAction) self.rotation_client_.wait_for_server() self.dive_client_.wait_for_server() self.linear_client_.wait_for_server() def rotate(self, yaw_deg: float): """Rotates the vehicle. Blocking call. :param yaw_deg: Angle to rotate in degrees, positive is for counterclockwise, negative - for clockwise. """ goal = self._create_rotate_goal(yaw_deg=yaw_deg) self._exec_action(self.rotation_client_, goal) def march(self, duration_ms: int, velocity: float, stopping_event: EventBase = None): """Moves the vehicle by march. Blocking call. Action execution will be canceled if stopping_event is triggered. :param duration_ms: Duration of movement in milliseconds. :param velocity: Velocity of movement, from 0.0 to 1.0, positive to move forward, negative - to backward movement. :param stopping_event: Event that stops movement. """ goal = self._create_march_goal(duration_ms, velocity) return self._exec_action(self.linear_client_, goal, stopping_event) def lag(self, duration_ms: int, velocity: float, stopping_event: EventBase = None): """Moves the vehicle by lag. Blocking call. Action execution will be canceled if stopping_event is triggered. :param duration_ms: Duration of the movement in milliseconds. :param velocity: Velocity of movement, from 0.0 to 1.0, positive to move right, negative - to move left (from vehicle point of view). :param stopping_event: Event that stops movement. """ goal = self._create_lag_goal(duration_ms, velocity) return self._exec_action(self.linear_client_, goal, stopping_event) def dive(self, depth_cm: int): """Dives the vehicle. Blocking call. :param depth_cm: Depth of diving in centimeters, must be positive. """ goal = stingray_movement_msgs.msg.DiveGoal(depth=depth_cm) self.dive_client_.send_goal(goal) self.dive_client_.wait_for_result() def stop(self): """Stops the vehicle. Blocking call. """ goal = self._create_stop_goal() self.linear_client_.send_goal(goal) self.linear_client_.wait_for_result() @staticmethod def _create_rotate_goal(yaw_deg: float): """Constructs action goal for rotation action server. This is a private method. :param yaw_deg: Angle to rotate in degrees, positive is for counterclockwise, negative - for clockwise. :return: Action goal. """ return stingray_movement_msgs.msg.RotateGoal(yaw=yaw_deg) @staticmethod def _create_march_goal(duration_ms: int, velocity: float): """Constructs action goal for linear movement server to move by march. This is a private method. :param duration_ms: Duration of movement in milliseconds. :param velocity: Velocity of movement, from 0.0 to 1.0, positive to move forward, negative - to backward movement. :return: Action goal. """ direction = stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_MARCH_FORWARD if velocity >= 0 \ else stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_MARCH_BACKWARDS goal = stingray_movement_msgs.msg.LinearMoveGoal(direction=direction, duration=duration_ms, velocity=abs(velocity)) return goal @staticmethod def _create_lag_goal(duration_ms: int, velocity: float): """Constructs action goal for linear movement server to move by lag. This is a private method. :param duration_ms: Duration of the movement in milliseconds. :param velocity: Velocity of movement, from 0.0 to 1.0, positive to move right, negative - to move left (from vehicle point of view). :return: Action goal. """ direction = stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_LAG_RIGHT if velocity >= 0 \ else stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_LAG_LEFT goal = stingray_movement_msgs.msg.LinearMoveGoal(direction=direction, duration=duration_ms, velocity=abs(velocity)) return goal @staticmethod def _create_stop_goal(): """Constructs action goal to stop any movement. This is a private method. :return: Action goal. """ return stingray_movement_msgs.msg.LinearMoveGoal( direction=stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_STOP, duration=0, velocity=0) @staticmethod 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 Explorer(): EXPLORING = 0 DONE = 1 def __init__(self, home_x=2.0, home_y=2.0): # flag to check for termination conditions self.is_running = True self.condition = Driver.NORMAL # initialized the "viewed " self.map = OccupancyGrid() # subscribe to the view_map self.map_sub = rospy.Subscriber("view_map", OccupancyGrid, self.update_map) # get file path # rospack = rospkg.RosPack() # dir_path = rospack.get_path("exercise1") # self.home_x = home_x # self.home_y = home_y # to_visit = list(reversed(orig_positions)) self.to_visit = copy.deepcopy(self.orig_positions) self.wait_time = 60.0 # used as seconds in autopilot function self.is_interrupted = False # create the action client and send goal self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server(rospy.Duration.from_sec(self.wait_time)) ## marking list of places to vist self.location_marker_pub = rospy.Publisher("destination_marker", Marker, queue_size=10) # self.create_all_markers() # important to register shutdown node with threads after thread have been created rospy.on_shutdown(self.shutdown) def def explore(self): ## plant flags ## don't stop until you visit everything def update_state(self): char = self.kb_thread.pop_c() if char == 'h': self.condition = Driver.HOME elif char == 'r': self.condition = Driver.RANDOMIZE elif char == '\x1b': self.condition = Driver.SHUTDOWN # print "shutdown condition" else: # self.condition = Driver.NORMAL pass return self.condition def cancel_goal(self): self.client.cancel_all_goals() def send_move_goal(self, loc): # create goal goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = loc[0] goal.target_pose.pose.position.y = loc[1] goal.target_pose.pose.position.z = loc[2] goal.target_pose.pose.orientation.x = loc[3] goal.target_pose.pose.orientation.y = loc[4] goal.target_pose.pose.orientation.z = loc[5] goal.target_pose.pose.orientation.w = loc[6] goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) return self.client # def do_something_interesting(self): # ''' Output a nice call phrase for the grader. # Assumes espeak is installed. BAD ASSUMPTION # ''' # sayings = ["I love you Will", "Great job Will", "Your a great teaching assistant", # "Am I home yet?", "When will this stop?", "Are you my mother?", # "Assimov books are great.", "Robot army on its way.", # "Your my friend.", "Kory deserves 100 percent."] # espeak_call_str = "espeak -s 120 \'{0}\'".format(choice(sayings)) # os.system(espeak_call_str) # def create_all_markers(self): # self.markers = [] # for i in xrange(len(self.orig_positions)): # loc = self.orig_positions[i] # marker_id = i # marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0,0,1,1)) # self.markers.append(marker) # # print "in create all markers", self.markers def mark_as_to_visit(self, loc, marker_id): marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0,1,0,1)) self.location_marker_pub.publish(marker) def mark_as_visited(self, loc, marker_id): marker = self.create_location_marker(loc, color, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0,1,1,1)) self.location_marker_pub.publish(marker) def create_location_marker(self, loc, marker_type=Marker.SPHERE, marker_action=Marker.ADD, marker_id=0, marker_color=ColorRGBA(1,0,0,1)): h = Header() h.frame_id = "map" h.stamp = rospy.Time.now() mark = Marker() mark.header = h mark.ns = "location_marker" mark.id = marker_id mark.type = marker_type mark.action = marker_action mark.scale = Vector3(0.25, 0.25, 0.25) mark.color = marker_color pose = Pose(Point(loc[0], loc[1], loc[2]), Quaternion(loc[3],loc[4],loc[5],loc[6])) mark.pose = pose return mark ### threaded way to do it def shutdown(self): self.client.cancel_all_goals() if __name__ == '__main__': rospy.init_node('explorer') msg = ''' Exploring map. ''' print msg explorer = Explorer() explorer.explore() rospy.spin()
class AcceptanceTestJointLimits(unittest.TestCase): """ Test that each joint can be moved exactly to its limits and not further. """ def setUp(self): """ Initialize a client for sending trajectories to the controller. """ self.client = SimpleActionClient(_CONTROLLER_ACTION_NAME, FollowJointTrajectoryAction) if not self.client.wait_for_server( timeout=rospy.Duration(_ACTION_SERVER_TIMEOUT_SEC)): self.fail('Timed out waiting for action server ' + _CONTROLLER_ACTION_NAME) self.joint_names = sorted(_JOINT_LIMITS_DEGREE.keys()) self.home_positions = [0] * len(self.joint_names) # read joint limits from urdf self.joint_velocity_limits = [0] * len(self.joint_names) robot = URDF.from_parameter_server("robot_description") for joint in robot.joints: if joint.name in self.joint_names: index = self.joint_names.index(joint.name) self.joint_velocity_limits[index] = joint.limit.velocity self.assertTrue(all(self.joint_velocity_limits)) def _ask_for_permission(self, test_name): s = raw_input('Perform ' + test_name + ' [(y)es, (n)o]?: ') if (s == "n"): print('\n\nSkip ' + test_name + '\n___TEST-END___\n') return 0 print('\n\nStart ' + test_name + '\n') return 1 def _current_joint_state_positions(self): """ Return the current joint state positions in the order given by self.joint_names. """ msg = JointState() positions = [] try: msg = rospy.wait_for_message(_JOINT_STATES_TOPIC_NAME, JointState, timeout=_WAIT_FOR_MESSAGE_TIMEOUT_SEC) except rospy.ROSException: self.fail('Could not retrieve message from topic ' + _JOINT_STATES_TOPIC_NAME) for name in self.joint_names: try: index = msg.name.index(name) positions.append(msg.position[index]) except ValueError: self.fail('Could not retrieve joint state position for ' + name) return positions def _check_joint_limits(self): """ Check if current joint positions are within the limits. """ positions = self._current_joint_state_positions() for i in range(len(self.joint_names)): position = positions[i] name = self.joint_names[i] limit = radians( _JOINT_LIMITS_DEGREE[name]) + _JOINT_POSITIONS_TOLERANCE self.assertGreater( position, -limit, 'Joint ' + name + ' violates lower limit. Position: ' + str(position)) self.assertLess( position, limit, 'Joint ' + name + ' violates upper limit. Position: ' + str(position)) def _drive_home(self): """ Move the robot to the home position. """ positions = self._current_joint_state_positions() diffs = [] for i in range(len(positions)): diffs.append(abs(positions[i] - self.home_positions[i])) if any(diff > _JOINT_POSITIONS_TOLERANCE for diff in diffs): self._execute_trajectory(self.home_positions) def _execute_trajectory(self, positions): """ Execute a single point trajectory given through joint positions (in the order given by self.joint_names). Return true on success and false otherwise. """ self.assertEqual(len(positions), len(self.joint_names)) # determine duration from distance, max velocity and velocity scaling current_positions = self._current_joint_state_positions() durations = [] for i in range(len(current_positions)): distance = abs(positions[i] - current_positions[i]) durations.append(distance / self.joint_velocity_limits[i]) duration = max(durations) / _VELOCITY_SCALE # construct goal traj_point = JointTrajectoryPoint() traj_point.positions = positions traj_point.time_from_start = rospy.Duration(duration) traj = JointTrajectory() traj.joint_names = self.joint_names traj.points.append(traj_point) goal = FollowJointTrajectoryGoal() goal.trajectory = traj # send to trajectory controller self.client.send_goal(goal) self.client.wait_for_result() # evaluate result result = self.client.get_result() success = (result.error_code == FollowJointTrajectoryResult.SUCCESSFUL) if not success: rospy.loginfo('Failure executing: ' + str(goal)) return success def _joint_limit_reaching_test(self, joint_name): """ Test if the robot can be commanded to move exactly to the limits Test Sequence: 1. Command a movement to the home position. 2. Command a movement to the lower limit. 3. Command a movement to the upper limit. 4. Command a movement to the home position. Expected Results: 1. Trajectory is executed successfully. 2. Trajectory is executed successfully. 3. Trajectory is executed successfully. 4. Trajectory is executed successfully. """ self._drive_home() index = self.joint_names.index(joint_name) limit = _JOINT_LIMITS_DEGREE[joint_name] lower_positions = [0] * len(self.joint_names) lower_positions[index] = -radians(limit) self.assertTrue(self._execute_trajectory(lower_positions)) upper_positions = [0] * len(self.joint_names) upper_positions[index] = radians(limit) self.assertTrue(self._execute_trajectory(upper_positions)) self._drive_home() def _joint_limit_overstepping_test(self, joint_name): """ Test if the robot does not overstep the limits Test Sequence: 1. Command a movement to the home position. 2. Command a movement overstepping the lower limit. 3. Command a movement overstepping the upper limit. 4. Command a movement to the home position. Expected Results: 1. Trajectory is executed successfully. 2. Trajectory execution is aborted and the robot does not overstep the limits. 3. Trajectory execution is aborted and the robot does not overstep the limits. 4. Trajectory is executed successfully. """ self._drive_home() index = self.joint_names.index(joint_name) limit = _JOINT_LIMITS_DEGREE[joint_name] lower_positions = [0] * len(self.joint_names) lower_positions[index] = -(radians(limit) + _JOINT_LIMIT_OVERSTEP) self.assertFalse(self._execute_trajectory(lower_positions)) self._check_joint_limits() upper_positions = [0] * len(self.joint_names) upper_positions[index] = radians(limit) + _JOINT_LIMIT_OVERSTEP self.assertFalse(self._execute_trajectory(upper_positions)) self._check_joint_limits() self._drive_home() def test_joint_limits_reaching(self): """ Perform all reaching tests. Before each test ask the user if he wants to skip it. """ for name in self.joint_names: if not name == _SELF_COLLISION_JOINT_NAME: if self._ask_for_permission('joint_limit_reaching_test for ' + name): self._joint_limit_reaching_test(name) def test_joint_limits_overstepping(self): """ Perform all overstepping tests. Before each test ask the user if he wants to skip it. """ for name in self.joint_names: if not name == _SELF_COLLISION_JOINT_NAME: if self._ask_for_permission( 'joint_limit_overstepping_test for ' + name): self._joint_limit_overstepping_test(name)
class MoveArm(object): # Variablen die den Schnitt unmittelbar beeinflussen offset_tip = 0.102 # Offset in cm (Dicke des Schneidbretts + Abstand zw. Fingerspitze und Klingenunterseite) blade_len = 0.05 # Laenge der Klinge ft_limit = 50 # Kraft Grenzwert ft_threshold = 25 # Kraft Schwellwert step_down = 0.01 # Standard Schnitttiefe def __init__(self, enabled=True): self.enabled = enabled self.client = SimpleActionClient('/qp_controller/command', ControllerListAction) rospy.loginfo('connecting to giskard') self.client.wait_for_server() rospy.loginfo('connected to giskard') self.tip = 'gripper_tool_frame' self.root = 'base_link' self.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', ] self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) # Subcriber fuer das Topic"/kms40/wrench_zeroed". Wenn Nachrichten empfangen werden, wird die Funktion # ft_callback aufgerufen. self.ft_sub = rospy.Subscriber("/kms40/wrench_zeroed", WrenchStamped, self.ft_callback) self.ft_list = [] #Liste fuer die gemessenen Kraftwerte. # Service, um den Offset des Kraftmomentensensors zu aktualisieren. # Der Service wartet auf ein Objekt vom Typ Trigger. self.reset_ft = rospy.ServiceProxy("/ft_cleaner/update_offset", Trigger) rospy.sleep(1) self.reset_ft.call(TriggerRequest()) #Trigger Objekt # Publisher fuer die Position des Endeffektors self.endeffector_pub = rospy.Publisher('endeffector_position', PoseStamped) def send_cart_goal(self, goal_pose, translation_weight=1, rotation_weight=1): if self.enabled: goal = ControllerListGoal() goal.type = ControllerListGoal.STANDARD_CONTROLLER # translation controller = Controller() controller.type = Controller.TRANSLATION_3D controller.tip_link = self.tip controller.root_link = self.root controller.goal_pose = goal_pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.05 controller.weight = translation_weight goal.controllers.append(controller) # rotation controller = Controller() controller.type = Controller.ROTATION_3D controller.tip_link = self.tip controller.root_link = self.root controller.goal_pose = goal_pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.2 controller.weight = rotation_weight goal.controllers.append(controller) self.client.send_goal(goal) result = self.client.wait_for_result(rospy.Duration(10)) print('finished in 10s?: {}'.format(result)) def relative_goal(self, position, orientation, translation_weight=1, rotation_weight=1): p = PoseStamped() p.header.frame_id = self.tip p.pose.position = Point(*position) p.pose.orientation = Quaternion(*orientation) self.send_cart_goal(p, translation_weight, rotation_weight) def send_joint_goal(self, joint_state): if self.enabled: goal = ControllerListGoal() goal.type = ControllerListGoal.STANDARD_CONTROLLER # translation controller = Controller() controller.type = Controller.JOINT controller.tip_link = self.tip controller.root_link = self.root controller.goal_state = joint_state controller.p_gain = 3 controller.enable_error_threshold = False controller.threshold_value = 0.01 controller.weight = 1.0 goal.controllers.append(controller) self.client.send_goal(goal) result = self.client.wait_for_result(rospy.Duration(10)) print('finished in 10s?: {}'.format(result)) def ft_callback(self, data): """ Callback fuer den Kraft-/Momentensensor :param data: Sensordaten :type: WrenchStamped """ # Abbruch der Bewegung, wenn gemessene Kraft das Limit ueberschreitet, um Sicherheitsabschaltung des Arms zuvorzukommen. if abs(data.wrench.force.z) > self.ft_limit: print("Stop") self.client.cancel_all_goals() # Lokalisation des Endeffektors trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip, rospy.Time()) p = PoseStamped() p.header = trans.header p.pose.position.x = trans.transform.translation.x p.pose.position.y = trans.transform.translation.y p.pose.position.z = trans.transform.translation.z p.pose.orientation.x = trans.transform.rotation.x p.pose.orientation.y = trans.transform.rotation.y p.pose.orientation.z = trans.transform.rotation.z p.pose.orientation.w = trans.transform.rotation.w self.endeffector_pub.publish(p) # Der absolute Kraftwert wird ausgelesen und zu einer Liste hinzugefuegt, die die Werte aneinander reiht, um # spaeter daraus eine Schneidestrategie abzuleiten. ft = abs(data.wrench.force.z) self.ft_list.append(ft) def move_tip_in_amp(self, x, y, z): """ Bewegung des Gripper Tool Frame in Bezug auf den Frame 'arm_mounting_plate' (amp). :type x,y,z: Distanz in Meter :param x: Bewegung entlang der x-Achse des Frame 'arm_mounting_plate' :param y: Bewegung entlang der y-Achse des Frame 'arm_mounting_plate' :param z: Bewegung entlang der z-Achse des Frame 'arm_mounting_plate' """ # Ermittelung der Position des Frames Gripper Tool in Bezug auf 'arm_mounting_plate'-Frame trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip, rospy.Time()) p = PoseStamped() p.header.frame_id = 'arm_mounting_plate' # Die soeben ermittelten Werte werden uebernommen und mit den Werten der gewuenschten Bewegung addiert. p.pose.position = trans.transform.translation p.pose.position.x += x p.pose.position.y += y p.pose.position.z += z p.pose.orientation = trans.transform.rotation cut.send_cart_goal(p) def distance2table(self): """ Berechnung des Abstandes von Klingenunterkante zu Oberflaeche der Schneidunterlage. :rtype: Distanz in Meter :return: Abstand zum Tisch """ # Abfrage der Position des Frames 'gripper_tool_frame' in Bezug auf 'arm_mounting_plate'. # Das Frame 'arm_mounting_plate' entspricht dabei der Tischoberkante. trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip, rospy.Time()) # Kalkulation des Abstandes von Klingen-Unterseite zum Schneidebrett mit Offset. distance2table = trans.transform.translation.z - self.offset_tip return distance2table def go_to_home(self): """ Definition der Standard Position. :return: """ print("Approach Home Pose") goal_joint_state = JointState() goal_joint_state.name = self.joint_names goal_joint_state.position = [ -2.417572323475973, -1.530511204396383, -1.6327641646014612, -1.5507991949664515, 1.5708668231964111, 1.509663701057434 ] self.send_joint_goal(goal_joint_state) print("Home Pose Approached") def align(self): """ Ausrichtung des Messers. Das Messer wird nach vorne gekippt, da die Klinge gebogen ist und sonst nicht buendig mit dem Schneidebrett abschliessen wuerde. Der tiefste Punkt der Klinge befindet sich so zentral ueber dem Objekt. Ebenfalls wird das Messer um die z-Achse gedreht, da die provisorische Halterung das Messer nicht perfekt ausgerichtet aufnimmt. Diese Werte sind bei Verwendung von anderem Messer und Halterung anzupassen. """ q = quaternion_from_euler(0, -0.15, 0.02, 'ryxz') cut.relative_goal([0, 0, 0], q) cut.move_tip_in_amp(-0.01, 0, 0) # Weitere Bewegungen des Endeffektors, die nicht beruecksichtigt wurden. # Einfache Schnittbewegung entlang der y-Achse (in Bezug auf gripper_tool_frame) bei gleicher Orientierung des Grippers # def straight_cut(self): # d2t = test.distance2table() # test.relative_goal([0,-d2t,0],[0,0,0,1]) # # Hackende Bewegung # def straight_chop(self): # max_step = 6 # for i in range(max_step): # test.relative_goal([0,-0.02,0],[0,0,0,1]) # test.relative_goal([0,0.01,0], [0, 0, 0, 1]) # # Saegende Bewegung # def saw(self): # max_step = 6 # for i in range(max_step): # test.relative_goal([0, -0.005, 0.05], [0, 0, 0, 1]) # test.relative_goal([0, -0.005, -0.05], [0, 0, 0, 1]) # # Einfache rollende Schnittbewegung # def roll_simple(self): # q = quaternion_from_euler(0, 0.3, 0, 'ryxz') # test.relative_goal([0,0,0],q,translation_weight=100) # Erhohung der Gewichtung der Translation, damit die Spitze genauer in Position bleibt # test.move_tip_in_amp(0, 0, -0.08) # q_1 = quaternion_from_euler(0, -0.3, 0, 'ryxz') # test.relative_goal([0, 0, 0],q_1,translation_weight=100) # # Erweiterte rollende Schnittbewegung # def roll_advanced(self): # q = quaternion_from_euler(0, 0.3, 0, 'ryxz') # test.relative_goal([0, 0, 0], q, translation_weight=100) # test.move_tip_in_amp(0, 0, -0.08) # test.move_tip_in_amp(-0.05, 0, 0) # q_1 = quaternion_from_euler(0, -0.3, 0, 'ryxz') # test.relative_goal([0, 0, 0], q_1, translation_weight=100) # # # def cross_cut(self): # max_step = 5 # for i in range(max_step): # q = quaternion_from_euler(0, 0.1, 0, 'ryxz') # test.relative_goal([0, 0, 0], q, translation_weight=100) # test.relative_goal([0, -0.01, 0.05], [0, 0, 0, 1]) # q_1 = quaternion_from_euler(0, -0.1, 0, 'ryxz') # test.relative_goal([0, 0, 0], q_1,translation_weight=100) # test.relative_goal([0, 0, -0.05], [0, 0, 0, 1]) def master_cut(self): """ Funktion fuer die Planung und Ausfuehrung des Schnitte :return: """ # Abfrage des aktuellen Abstands von Klinge zu Schneidebrett. d2t = cut.distance2table() # Solange dieser Abstand positiv ist, also sich das Messer oberhalb des Schneidbretts befindet, wird geschnitten. while d2t > 0: # Aufruf der Funktion, die die Bewegung unter Beruecksichtig verschiedener Paramenter berechnet und zurueckgibt. down, side, final = cut.calc_move() # Bewegung, wenn der gemessene F/T Wert den Schwellwert nicht ueberschritten hat. In diesem Fall erfolgt die # Bewegung rein entlang der z-Achse. if side == 0: cut.move_tip_in_amp(0, 0, -down) # Wenn F/T-Wert den Grenzwert ueberschreitet, kommt eine Bewegung in x Richtung dazu. # Dabei wird zunaechst die Klinge ohne Bewegung zurueck gefahren, um von der vollen Klingenlaenge # zu profitieren. Anschliessend erfolgt eine diagonale Schnittbewegung ueber die gesamte Klingenlaenge. # Abschliessend eine weitere diagonale Bewegung, um wieder in die Ausgangsposition (x-Achse) zu gelangen. else: cut.move_tip_in_amp(-side, 0, -(1 / 4) * down) cut.move_tip_in_amp(2 * side, 0, -(2 / 4) * down) cut.move_tip_in_amp(-side, 0, -(1 / 4) * down) # Wenn die letze Bewegung ausgefuehrte wurde (also Final == True von calc_move() zurueckgegeben wird), # wird die Funktion beendet. Der Schnittvorgang wird mit Bewegungen entlag der x-Achse abgeschlossen, # um sicherzustellen, dass das Objekt in Gaenze durchtrennt wird. Abschliessend faehrt das Messer seitlich # entlang der y-Achse um das abgetrennte Stueck zu separieren. if final == True: print("Final") cut.move_tip_in_amp(-self.blade_len, 0, 0) cut.move_tip_in_amp(self.blade_len * 1.5, 0, 0) cut.move_tip_in_amp(-self.blade_len / 2, 0, 0.005) cut.move_tip_in_amp(0, 0.05, 0) print("Cut Finished") return # Funktion um die Schnittbewegung zu berechnen def calc_move(self): """ Return of three values necessary for cut-move execution. :return: 1.value:cutting depth; 2.value: lateral move; 3.value: final cut :type: (float,float,bool) """ # Init final = False # Abfrage des maximalen F/T-Werts aus der letzten Bewegung cur_ft = cut.max_ft() # cur_ft = self.ft_threshold # Abfrage des aktuellen Abstands von Klingenunterseite zu Schneidebrett d2t = cut.distance2table() print("Distance to Table %s" % d2t) print("Current FT %s" % cur_ft) # Wenn der Kraftwert den Schwellwert unterschreitet, wird nur entlang der z-Achse geschnitten if cur_ft < self.ft_threshold: down = self.step_down side = 0 # Wenn die Schritttiefe kleiner als der Abstand zur Oberflaeche ist, # wird die Schritttiefe der naechsten Bewegung auf die verbleibende Distanz gesetzt, # um Kollisionen mit dem Tisch zu vermeiden. if d2t <= down: down = d2t final = True else: # Berechnung der Bewegung, wenn der Kraftschwellwert ueberschritten wird. # Je hoeher der gemessene Kraftwert, desto geringer die Schnitttiefe. # Die maximale berechnete Schnitttiefe entspricht der Standardschnitttiefe, # wenn die Kraft dem Schwellwert entspricht. down = (self.ft_threshold / cur_ft) * (self.step_down) # Setzen einer Mindestschnitttiefe if down < 0.001: down = 0.001 # Wenn die berechnete Schrittweite kleiner als der Abstand zur Oberflaeche, # wird die Schrittweite der naechsten Bewegung entsprechend angepasst, # um Kollisionen mit dem Tisch zu vermeiden. if d2t <= down: down = d2t final = True # Letzte Bewegung # Die seitliche Bewegung entspricht der Haelfte der Klingenlaenge. side = self.blade_len / 2 print("Side %s" % side) print("Down %s" % down) return (down, side, final) def max_ft(self): """ Funktion um den maximalen F/T waehrend der vergangenen Bewegung auszulesen und zurueckzugeben. :return: Maximale Kraft waehrend der letzten Bewegung """ # Abfrage des max. F/T aus der Liste der F/T Werte ft_max = max(self.ft_list) # Nach dem der Wert zwischengespeichert worden ist, wird die Liste fuer den naechsten Durchlauf geleert. self.ft_list = [] print("Max. FT: %s" % ft_max) return (ft_max)
#!/usr/bin/env python import sys import rospy from actionlib import SimpleActionClient from pal_interaction_msgs.msg import TtsAction, TtsGoal from std_msgs.msg import String def talk_to_me(words): #creates the ts goal and sends it off to be spoken ttg = TtsGoal() ttg.rawtext.text = words.data ttg.rawtext.lang_id = "en_GB" client.send_goal_and_wait(ttg) #start the node rospy.init_node('tiago_tts') # Connect to the text-to-speech action server client = SimpleActionClient('/tts', TtsAction) client.wait_for_server() #Subscribe to get what we want to say rospy.Subscriber('/hearts/tts', String, talk_to_me) rospy.spin() #if __name__ == '__main__':
class PickAruco(object): def __init__(self): rospy.loginfo("Initalizing...") self.bridge = CvBridge() self.tfBuffer = tf2_ros.Buffer() self.tf_l = tf2_ros.TransformListener(self.tfBuffer) rospy.loginfo("Waiting for /pickup_pose AS...") self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction) time.sleep(1.0) if not self.pick_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /pickup_pose AS") exit() rospy.loginfo("Waiting for /place_pose AS...") self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction) self.place_as.wait_for_server() rospy.loginfo("Setting publishers to torso and head controller...") self.torso_cmd = rospy.Publisher('/torso_controller/command', JointTrajectory, queue_size=1) self.head_cmd = rospy.Publisher('/head_controller/command', JointTrajectory, queue_size=1) self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose', PoseStamped, queue_size=1, latch=True) rospy.loginfo("Waiting for '/play_motion' AS...") self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction) if not self.play_m_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /play_motion AS") exit() rospy.loginfo("Connected!") rospy.sleep(1.0) rospy.loginfo("Done initializing PickAruco.") def strip_leading_slash(self, s): return s[1:] if s.startswith("/") else s def pick_aruco(self, string_operation): self.prepare_robot() rospy.sleep(2.0) rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection") aruco_pose = rospy.wait_for_message('/Perception/Pick_Pose', PoseStamped) aruco_pose.header.frame_id = self.strip_leading_slash( aruco_pose.header.frame_id) rospy.loginfo("Got: " + str(aruco_pose)) rospy.loginfo("spherical_grasp_gui: Transforming from frame: " + aruco_pose.header.frame_id + " to 'base_footprint'") ps = PoseStamped() ps.pose.position = aruco_pose.pose.position ps.header.stamp = self.tfBuffer.get_latest_common_time( "base_footprint", aruco_pose.header.frame_id) ps.header.frame_id = aruco_pose.header.frame_id transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: transform = self.tfBuffer.lookup_transform( "base_footprint", ps.header.frame_id, rospy.Time(0)) aruco_ps = do_transform_pose(ps, transform) transform_ok = True except tf2_ros.ExtrapolationException as e: rospy.logwarn( "Exception on transforming point... trying again \n(" + str(e) + ")") rospy.sleep(0.01) ps.header.stamp = self.tfBuffer.get_latest_common_time( "base_footprint", aruco_pose.header.frame_id) pick_g = PickUpPoseGoal() if string_operation == "pick": rospy.loginfo("Setting cube pose based on bottle detection") pick_g.object_pose.pose.position = aruco_ps.pose.position # pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) rospy.loginfo("aruco pose in base_footprint:" + str(pick_g)) pick_g.object_pose.header.frame_id = 'base_footprint' pick_g.object_pose.pose.orientation.w = 1.0 self.detected_pose_pub.publish(pick_g.object_pose) rospy.loginfo("Gonna pick:" + str(pick_g)) self.pick_as.send_goal_and_wait(pick_g) rospy.loginfo("Done!") result = self.pick_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr("Failed to pick, not trying further") return # Move torso to its maximum height self.lift_torso() # Raise arm #rospy.loginfo("Moving arm to a safe pose") #pmg = PlayMotionGoal() # pmg.motion_name = 'pick_final_pose' #pmg.skip_planning = False #self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Raise object done.") # Place the object back to its position #rospy.loginfo("Gonna place near where it was") #pick_g.object_pose.pose.position.z += 0.05 #self.place_as.send_goal_and_wait(pick_g) #rospy.loginfo("Done!") def place_aruco(self, string_operation): rospy.sleep(2.0) rospy.loginfo("spherical_grasp_gui: Waiting for an put position") aruco_pose = rospy.wait_for_message('/Perception/Place_Pose', PoseStamped) aruco_pose.header.frame_id = self.strip_leading_slash( aruco_pose.header.frame_id) aruco_pose.pose.position.x = 0.0 aruco_pose.pose.position.y = 0.0 aruco_pose.pose.position.z = 0.0 rospy.loginfo("Got: " + str(aruco_pose)) rospy.loginfo("spherical_grasp_gui: Transforming from frame: " + aruco_pose.header.frame_id + " to 'base_footprint'") ps = PoseStamped() ps.pose.position = aruco_pose.pose.position #ps.pose.position.x = 0.9 #ps.pose.position.y = 0.0 #ps.pose.position.z = 0.8 ps.header.stamp = self.tfBuffer.get_latest_common_time( "base_footprint", aruco_pose.header.frame_id) ps.header.frame_id = aruco_pose.header.frame_id transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: transform = self.tfBuffer.lookup_transform( "base_footprint", ps.header.frame_id, rospy.Time(0)) aruco_ps = do_transform_pose(ps, transform) transform_ok = True except tf2_ros.ExtrapolationException as e: rospy.logwarn( "Exception on transforming point... trying again \n(" + str(e) + ")") rospy.sleep(0.01) ps.header.stamp = self.tfBuffer.get_latest_common_time( "base_footprint", aruco_pose.header.frame_id) pick_g = PickUpPoseGoal() if string_operation == "place": rospy.loginfo("Setting cube pose based on bottle detection") pick_g.object_pose.pose.position.x = aruco_ps.pose.position.x + 0.95 pick_g.object_pose.pose.position.y = aruco_ps.pose.position.y pick_g.object_pose.pose.position.z = aruco_ps.pose.position.z + 1.0 # pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) rospy.loginfo("place pose in base_footprint:" + str(pick_g)) pick_g.object_pose.header.frame_id = 'base_footprint' pick_g.object_pose.pose.orientation.w = 1.0 self.detected_pose_pub.publish(pick_g.object_pose) rospy.loginfo("Gonna place:" + str(pick_g)) self.place_as.send_goal_and_wait(pick_g) rospy.loginfo("Done!") # Place the object back to its position # rospy.loginfo("Gonna place near where it was") # pick_g.object_pose.pose.position.z += 0.05 # self.place_as.send_goal_and_wait(pick_g) # rospy.loginfo("Done!") def lift_torso(self): rospy.loginfo("Moving torso up") jt = JointTrajectory() jt.joint_names = ['torso_lift_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0.34] jtp.time_from_start = rospy.Duration(2.5) jt.points.append(jtp) self.torso_cmd.publish(jt) def lower_head(self): rospy.loginfo("Moving head down") jt = JointTrajectory() jt.joint_names = ['head_1_joint', 'head_2_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0.0, -0.45] #0.0, -0.75 jtp.time_from_start = rospy.Duration(2.0) jt.points.append(jtp) self.head_cmd.publish(jt) rospy.loginfo("Done.") def prepare_robot(self): rospy.loginfo("Unfold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'pregrasp' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done.") self.lower_head() rospy.loginfo("Robot prepared.")
class PickAndPlaceServer(object): def __init__(self): rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Succesfully connected.") rospy.loginfo("Connecting to place AS") self.place_ac = SimpleActionClient('/place', PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Succesfully connected.") self.scene = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the object size self.object_height = rospy.get_param('~object_height') self.object_width = rospy.get_param('~object_width') self.object_depth = rospy.get_param('~object_depth') # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param( '~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn( "Didn't find any links to allow contacts... at param ~links_to_allow_contact" ) else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer('/pickup_pose', PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer('/place_pose', PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start() def pick_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.grasp_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.pick_as.set_aborted(p_res) else: self.pick_as.set_succeeded(p_res) def place_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.place_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.place_as.set_aborted(p_res) else: self.place_as.set_succeeded(p_res) def wait_for_planning_scene_object(self, object_name='part'): rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...") gps_req = GetPlanningSceneRequest() gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: # This call takes a while when rgbd sensor is set gps_resp = self.scene_srv.call(gps_req) # check if 'part' is in the answer for collision_obj in gps_resp.scene.world.collision_objects: if collision_obj.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo("'" + object_name + "'' is in scene!") def grasp_object(self, object_pose): rospy.loginfo("Removing any previous 'part' object") self.scene.remove_attached_object("arm_tool_link") self.scene.remove_world_object("part") self.scene.remove_world_object("table") rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) # Removing is fast rospy.loginfo("Adding new 'part' object") rospy.loginfo("Object pose: %s", object_pose.pose) #Add object description in scene self.scene.add_box( "part", object_pose, (self.object_depth, self.object_width, self.object_height)) rospy.loginfo("Second%s", object_pose.pose) self.add_table(copy.deepcopy(object_pose)) # compute grasps possible_grasps = self.sg.create_grasps_from_object_pose(object_pose) self.pickup_ac goal = createPickupGoal("arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.logdebug("Using torso result: " + str(result)) rospy.loginfo("Pick result: " + str(moveit_error_dict[result.error_code.val])) return result.error_code.val def place_object(self, object_pose): rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) self.add_table(copy.deepcopy(object_pose)) possible_placings = self.sg.create_placings_from_object_pose( object_pose) # Try only with arm rospy.loginfo("Trying to place using only arm") goal = createPlaceGoal(object_pose, possible_placings, "arm", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo(str(moveit_error_dict[result.error_code.val])) if str(moveit_error_dict[result.error_code.val]) != "SUCCESS": rospy.loginfo("Trying to place with arm and torso") # Try with arm and torso goal = createPlaceGoal(object_pose, possible_placings, "arm_torso", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.logerr(str(moveit_error_dict[result.error_code.val])) # print result rospy.loginfo("Result: " + str(moveit_error_dict[result.error_code.val])) rospy.loginfo("Removing previous 'part' object") self.scene.remove_world_object("part") return result.error_code.val def add_table(self, object_pose): table_pose = object_pose table_pose.pose.orientation = Quaternion() #define a virtual table below the object table_height = object_pose.pose.position.z - self.object_width / 2 + 0.075 table_width = 1.8 table_depth = 1.0 table_pose.pose.position.x += 0.35 #0.3 table_pose.pose.position.z += -( 2 * self.object_width) / 2 - table_height / 2 #table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table self.scene.add_box("table", table_pose, (table_depth, table_width, table_height)) # # We need to wait for the object part to appear #self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("table")
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 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 Effector(object): """ Controls the end effector planner. """ def __init__(self, verbose=False): self.verbose = verbose self.client = Client(topics.move_end_effector_controller, MoveEndEffectorAction) goal = MoveEndEffectorGoal(command=MoveEndEffectorGoal.TEST) func = partial(retry_action, client=self.client, goal=goal, timeout=20, msg='Test end effector ') setattr(self, 'test', func) goal = MoveEndEffectorGoal(command=MoveEndEffectorGoal.PARK) func = partial(retry_action, client=self.client, goal=goal, timeout=20, msg='Park end effector ') setattr(self, 'park', func) def cancel_all_goals(self): log.debug('Waiting for the EndEffector action server...') self.client.wait_for_server() log.info('Canceling all goals on EndEffector.') self.client.cancel_all_goals() sleep(3) def point_to(self, target, center='/front_picam_frame'): """ Points end effector to a target. :param target: The victim frame ID. :param center: The center of the frame the we will use. """ goal = MoveEndEffectorGoal() goal.command = MoveEndEffectorGoal.TRACK goal.point_of_interest = target goal.center_point = center log.debug('Waiting for the EndEffector action server.') self.client.wait_for_server() log.info('Sending TRACK goal.') self.client.send_goal(goal) def slowly_point_to(self, target, center='/front_picam_frame'): """ Points end effector to a target, slow enough so the image is staying still. :param target: The victim frame ID. :param center: The center of the frame the we will use. """ goal = MoveEndEffectorGoal() goal.command = MoveEndEffectorGoal.LAX_TRACK goal.point_of_interest = target goal.center_point = center log.debug('Waiting for the EndEffector action server.') self.client.wait_for_server() log.info('Sending LAX TRACK goal.') self.client.send_goal(goal) sleep(5) def scan(self): """ The end effector starts scanning. """ goal = MoveEndEffectorGoal() goal.command = MoveEndEffectorGoal.SCAN log.debug('Waiting for the EndEffector action server.') self.client.wait_for_server() log.info('Sending SCAN goal.') self.client.send_goal(goal)
class SmartGrasper(object): """ This is the helper library to easily access the different functionalities of the simulated robot from python. """ __last_joint_state = None __current_model_name = "cricket_ball" __path_to_models = "/root/.gazebo/models/" def __init__(self): """ This constructor initialises the different necessary connections to the topics and services and resets the world to start in a good position. """ rospy.init_node("smart_grasper") self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.__joint_state_cb, queue_size=1) rospy.wait_for_service("/gazebo/get_model_state", 10.0) rospy.wait_for_service("/gazebo/reset_world", 10.0) self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty) self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) rospy.wait_for_service("/gazebo/pause_physics") self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty) rospy.wait_for_service("/gazebo/unpause_physics") self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) rospy.wait_for_service("/controller_manager/switch_controller") self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController) rospy.wait_for_service("/gazebo/set_model_configuration") self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration) rospy.wait_for_service("/gazebo/delete_model") self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel) rospy.wait_for_service("/gazebo/spawn_sdf_model") self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service('/get_planning_scene', 10.0) self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True) self.arm_commander = MoveGroupCommander("arm") self.hand_commander = MoveGroupCommander("hand") self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") self.reset_world() def reset_world(self): """ Resets the object poses in the world and the robot joint angles. """ self.__switch_ctrl.call(start_controllers=[], stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"], strictness=SwitchControllerRequest.BEST_EFFORT) self.__pause_physics.call() joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3'] joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0] self.__set_model.call(model_name="smart_grasping_sandbox", urdf_param_name="robot_description", joint_names=joint_names, joint_positions=joint_positions) timer = Timer(0.0, self.__start_ctrl) timer.start() time.sleep(0.1) self.__unpause_physics.call() self.__reset_world.call() def get_object_pose(self): """ Gets the pose of the ball in the world frame. @return The pose of the ball. """ return self.__get_pose_srv.call(self.__current_model_name, "world").pose def get_tip_pose(self): """ Gets the current pose of the robot's tooltip in the world frame. @return the tip pose """ return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose def move_tip_absolute(self, target): """ Moves the tooltip to the absolute target in the world frame @param target is a geometry_msgs.msg.Pose @return True on success """ self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([target]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): """ Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. @return True on success """ transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw), PyKDL.Vector(-x, -y, -z)) tip_pose = self.get_tip_pose() tip_pose_kdl = posemath.fromMsg(tip_pose) final_pose = toMsg(tip_pose_kdl * transform) self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([final_pose]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def send_command(self, command, duration=0.2): """ Send a dictionnary of joint targets to the arm and hand directly. @param command: a dictionnary of joint names associated with a target: {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0} @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0 """ hand_goal = None arm_goal = None for joint, target in command.items(): if "H1" in joint: if not hand_goal: hand_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) hand_goal.trajectory.points.append(point) hand_goal.trajectory.joint_names.append(joint) hand_goal.trajectory.points[0].positions.append(target) else: if not arm_goal: arm_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) arm_goal.trajectory.points.append(point) arm_goal.trajectory.joint_names.append(joint) arm_goal.trajectory.points[0].positions.append(target) if arm_goal: self.__arm_traj_client.send_goal_and_wait(arm_goal) if hand_goal: self.__hand_traj_client.send_goal_and_wait(hand_goal) def get_current_joint_state(self): """ Gets the current state of the robot. @return joint positions, velocity and efforts as three dictionnaries """ joints_position = {n: p for n, p in zip(self.__last_joint_state.name, self.__last_joint_state.position)} joints_velocity = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.velocity)} joints_effort = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.effort)} return joints_position, joints_velocity, joints_effort def open_hand(self): """ Opens the hand. @return True on success """ self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def close_hand(self): """ Closes the hand. @return True on success """ self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def check_fingers_collisions(self, enable=True): """ Disables or enables the collisions check between the fingers and the objects / table @param enable: set to True to enable / False to disable @return True on success """ objects = ["cricket_ball__link", "drill__link", "cafe_table__link"] while self.__pub_planning_scene.get_num_connections() < 1: rospy.loginfo("waiting for someone to subscribe to the /planning_scene") rospy.sleep(0.1) request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.__get_planning_scene(request) acm = response.scene.allowed_collision_matrix for object_name in objects: if object_name not in acm.entry_names: # add object to allowed collision matrix acm.entry_names += [object_name] for row in range(len(acm.entry_values)): acm.entry_values[row].enabled += [False] new_row = deepcopy(acm.entry_values[0]) acm.entry_values += {new_row} for index_entry_values, entry_values in enumerate(acm.entry_values): if "H1_F" in acm.entry_names[index_entry_values]: for index_value, _ in enumerate(entry_values.enabled): if acm.entry_names[index_value] in objects: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True elif acm.entry_names[index_entry_values] in objects: for index_value, _ in enumerate(entry_values.enabled): if "H1_F" in acm.entry_names[index_value]: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.__pub_planning_scene.publish(planning_scene_diff) rospy.sleep(1.0) return True def pick(self): """ Does its best to pick the ball. """ rospy.loginfo("Moving to Pregrasp") self.open_hand() time.sleep(0.1) ball_pose = self.get_object_pose() ball_pose.position.z += 0.5 #setting an absolute orientation (from the top) quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) ball_pose.orientation.x = quaternion[0] ball_pose.orientation.y = quaternion[1] ball_pose.orientation.z = quaternion[2] ball_pose.orientation.w = quaternion[3] self.move_tip_absolute(ball_pose) time.sleep(0.1) rospy.loginfo("Grasping") self.move_tip(y=-0.164) time.sleep(0.1) self.check_fingers_collisions(False) time.sleep(0.1) self.close_hand() time.sleep(0.1) rospy.loginfo("Lifting") for _ in range(5): self.move_tip(y=0.01) time.sleep(0.1) self.check_fingers_collisions(True) def swap_object(self, new_model_name): """ Replaces the current object with a new one.Replaces @new_model_name the name of the folder in which the object is (e.g. beer) """ try: self.__delete_model(self.__current_model_name) except: rospy.logwarn("Failed to delete: " + self.__current_model_name) try: sdf = None initial_pose = Pose() initial_pose.position.x = 0.15 initial_pose.position.z = 0.82 with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model: sdf = model.read() res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world") rospy.logerr( "RES: " + str(res) ) self.__current_model_name = new_model_name except: rospy.logwarn("Failed to delete: " + self.__current_model_name) def __compute_arm_target_for_ball(self): ball_pose = self.get_object_pose() # come at it from the top arm_target = ball_pose arm_target.position.z += 0.5 quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) arm_target.orientation.x = quaternion[0] arm_target.orientation.y = quaternion[1] arm_target.orientation.z = quaternion[2] arm_target.orientation.w = quaternion[3] return arm_target def __pre_grasp(self, arm_target): self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() self.hand_commander.execute(plan, wait=True) for _ in range(10): self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([arm_target]) plan = self.arm_commander.plan() if self.arm_commander.execute(plan): return True def __grasp(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z -= 0.12 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False self.hand_commander.attach_object("cricket_ball__link") def __lift(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z += 0.1 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False def __start_ctrl(self): rospy.loginfo("STARTING CONTROLLERS") self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], stop_controllers=[], strictness=1) def __joint_state_cb(self, msg): self.__last_joint_state = msg
class GetCoffee(object): def __init__(self): self.ask_timeout = 10 #number of seconds to wait for an answer re coffee self.get_coffee_timeout = 60.0 * 2 #number of seconds to wait for a coffee at the machine self.deliver_timeout = 30 #number of seconds to wait for the person to retrieve the coffee self.coffee_wp = 'WayPoint8' #location of the coffee machine self.people = ['John'] #people to serve self.initial_location = { 'John': { 'WayPoint1': 0.8, 'WayPoint2': 0.05 } } #distribution of people's location in the environment self.wants_coffee = { 'John': 0.7 } #probability of people wanting coffee # define the mdp action client self.mdp_client = SimpleActionClient( 'mdp_plan_exec/execute_policy_extended', ExecutePolicyExtendedAction) self.mdp_client.wait_for_server() # build planning problem model = self.build_mdp_spec() # send goal to mdp action server self.mdp_client.send_goal(ExecutePolicyExtendedGoal(spec=model)) self.mdp_client.wait_for_result() def build_mdp_spec(self): mdp_vars = [] #the state features other than robot location actions = [] #the actions other than navigation for person in self.people: #looping for each person asked_var_name = 'asked_' + person asked_var = MdpStateVar( name=asked_var_name, init_val=0, min_range=0, max_range=1 ) #boolean state feature representing whether the person was asked whether they want coffee. This will only become true after the person replies to the question at a certain location. mdp_vars += [asked_var] for ( possible_init_location, prob ) in self.initial_location[person].items( ): #looping for each possible location of this person and corresponding probability wants_var_name = person + 'wants_at_' + possible_init_location wants_at_var = MdpStateVar( name=wants_var_name, init_val=-1, min_range=-1, max_range=1 ) #state feature representing whether person wants coffee at this location. -1 represents the robot has not asked, 0 represents the person responded no coffee, 1 represents the person responding yes to coffee. In this case we need to remember the location to bring the coffee back, so we add a feature for each possible (person, location) pair. mdp_vars += [wants_at_var] #action encoding of asking person whether they want coffee, at possible_init_location ask_action = MdpAction( name="ask_" + person + '_at_' + possible_init_location, #the name of the action to be used in the MDP model action_server= 'ask_question_server', #the actionlib server implementing the behaviour. This will be called by the policy executor waypoints=[ possible_init_location ], #the waypoint(s) where the behavior can be executed pre_conds=[ StringIntPair(wants_var_name, -1), StringIntPair(asked_var_name, 0) ], #preconditions for this action: it must be unknown whether the person is at this location and the person must not have been asked whether they want coffee outcomes=[]) #will be filled later #adding arguments to be passed to the 'ask_question_server' action server during execution. These are in line with the definition of the action msg for the action server located at ros_ws/src/isr_monarch_robot/mbot_task_planning/mbot_action_msgs/action/AskQuestion.action. mu.add_string_argument(ask_action, "Hi " + person + ". Would you like some coffee?" ) #What the robot should say. mu.add_string_argument(ask_action, "yes") #Answer to trigger success mu.add_float_argument( ask_action, self.ask_timeout) #waiting for reply timeout mu.add_string_argument( ask_action, "OK, I will bring it as soon as I can." ) #What to say if the person was here and said yes to coffee mu.add_string_argument( ask_action, "Good call, caffeine is bad for you and I have more important things to do." ) #what to say if the person was here but said no to coffee mu.add_string_argument(ask_action, "Looks like no one is here." ) #what to say if person is not here #the outcomes for the asking action person_here_prob = self.initial_location[person][ possible_init_location] #probability the person is in location possible_init_location #person was not in possible_init_location no_person_outcome = MdpActionOutcome( probability=1 - person_here_prob, #probability of this outcome, to be used in the MDP model waypoint= possible_init_location, #location of the robot after executing the action. In this case the robot does not move, so it ends in the same place. This will be the case for all actions in this example. post_conds=[ StringIntPair(wants_var_name, 0) ], #state feature update for this outcome. the state feature representing whether the robot the person wants coffee at this location becomes is there becomes 0. duration_probs=[ 1.0 ], #duration probabilities. This does not apply to tis example and will always be one. durations=[ self.ask_timeout / 2 ], #expected time for action execution, to be used in the MDP model. We set expected times to half the timeout. One could also learn these from experience. status=[ GoalStatus.SUCCEEDED ], #After executing the action in the robot, we check if the actionlib status matched a value in this array. If so, we perform an extra check using the result attribute below. result=[ StringTriple('timeout', MdpActionOutcome.BOOL_TYPE, 'True') ] ) #After executing the action in the robot, if the check above succeeded, we check if the actionlib result matched the conditions given here. In this case, we check whether the attribute 'timeout' of the result object is equal to the boolean True. # person is in possible_init_location and wants coffee wants_coffee_outcome = MdpActionOutcome( probability=person_here_prob * self.wants_coffee[person], waypoint=possible_init_location, post_conds=[ StringIntPair(asked_var_name, 1), StringIntPair(wants_var_name, 1) ], #in this case, 2 state features are updated. The person has been asked about coffee, and the person wants coffee. duration_probs=[1.0], durations=[self.ask_timeout / 2], status=[GoalStatus.SUCCEEDED], result=[ StringTriple('timeout', MdpActionOutcome.BOOL_TYPE, 'False'), StringTriple('response', MdpActionOutcome.BOOL_TYPE, 'True') ] ) # for this outcome to occur during execution, the timeout attribute of the action result must be False, and the response outcome must be True, meaning the person responded 'yes' doesnt_want_coffee_outcome = MdpActionOutcome( probability=person_here_prob * (1 - self.wants_coffee[person]), waypoint=possible_init_location, post_conds=[ StringIntPair(asked_var_name, 1), StringIntPair(wants_var_name, 0) ], duration_probs=[1.0], durations=[self.ask_timeout / 2], status=[GoalStatus.SUCCEEDED], result=[ StringTriple('timeout', MdpActionOutcome.BOOL_TYPE, 'False'), StringTriple('response', MdpActionOutcome.BOOL_TYPE, 'False') ]) ask_action.outcomes = [ no_person_outcome, wants_coffee_outcome, doesnt_want_coffee_outcome ] actions += [ask_action] ltl_task = "(F (asked_John = 1))" #hard coded task #build the mdp spec mdp_spec = MdpDomainSpec(vars=mdp_vars, actions=actions, ltl_task=ltl_task) return mdp_spec
class MyFrame(wx.Frame): def __init__(self, parent, id): the_size = (700, 720) # height from 550 change to 700 wx.Frame.__init__(self, parent, id, 'Elfin Control Panel', pos=(250, 100)) self.panel = wx.Panel(self) font = self.panel.GetFont() font.SetPixelSize((12, 24)) self.panel.SetFont(font) self.listener = tf.TransformListener() self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander('elfin_arm') self.controller_ns = 'elfin_arm_controller/' self.elfin_driver_ns = 'elfin_ros_control/elfin/' self.elfin_IO_ns = 'elfin_ros_control/elfin/io_port1/' # 20201126: add IO ns self.call_read_do_req = ElfinIODReadRequest() self.call_read_di_req = ElfinIODReadRequest() self.call_read_do_req.data = True self.call_read_di_req.data = True self.call_read_do = rospy.ServiceProxy(self.elfin_IO_ns + 'read_do', ElfinIODRead) self.call_read_di = rospy.ServiceProxy(self.elfin_IO_ns + 'read_di', ElfinIODRead) # 20201126: add service for write_do self.call_write_DO = rospy.ServiceProxy(self.elfin_IO_ns + 'write_do', ElfinIODWrite) self.elfin_basic_api_ns = 'elfin_basic_api/' self.joint_names = rospy.get_param(self.controller_ns + 'joints', []) self.ref_link_name = self.group.get_planning_frame() self.end_link_name = self.group.get_end_effector_link() self.ref_link_lock = threading.Lock() self.end_link_lock = threading.Lock() self.DO_btn_lock = threading.Lock() # 20201208: add the threading lock self.DI_show_lock = threading.Lock() self.js_display = [0] * 6 # joint_states self.jm_button = [0] * 6 # joints_minus self.jp_button = [0] * 6 # joints_plus self.js_label = [0] * 6 # joint_states self.ps_display = [0] * 6 # pcs_states self.pm_button = [0] * 6 # pcs_minus self.pp_button = [0] * 6 # pcs_plus self.ps_label = [0] * 6 # pcs_states # 20201208: add the button array self.DO_btn_display = [0] * 4 # DO states self.DI_display = [0] * 4 # DI states self.LED_display = [0] * 4 # LED states self.End_btn_display = [0] * 4 # end button states self.btn_height = 370 # 20201126: from 390 change to 370 self.btn_path = os.path.dirname( os.path.realpath(__file__)) # 20201209: get the elfin_gui.py path btn_lengths = [] self.DO_DI_btn_length = [ 0, 92, 157, 133 ] # 20201209: the length come from servo on, servo off, home, stop button self.btn_interstice = 22 # 20201209: come from btn_interstice self.display_init() self.key = [] self.DO_btn = [0, 0, 0, 0, 0, 0, 0, 0] # DO state, first four bits is DO, the other is LED self.DI_show = [ 0, 0, 0, 0, 0, 0, 0, 0 ] # DI state, first four bits is DI, the other is the end button self.power_on_btn = wx.Button(self.panel, label=' Servo On ', name='Servo On', pos=(20, self.btn_height)) btn_lengths.append(self.power_on_btn.GetSize()[0]) btn_total_length = btn_lengths[0] self.power_off_btn = wx.Button(self.panel, label=' Servo Off ', name='Servo Off') btn_lengths.append(self.power_off_btn.GetSize()[0]) btn_total_length += btn_lengths[1] self.reset_btn = wx.Button(self.panel, label=' Clear Fault ', name='Clear Fault') btn_lengths.append(self.reset_btn.GetSize()[0]) btn_total_length += btn_lengths[2] self.home_btn = wx.Button(self.panel, label='Home', name='home_btn') btn_lengths.append(self.home_btn.GetSize()[0]) btn_total_length += btn_lengths[3] self.stop_btn = wx.Button(self.panel, label='Stop', name='Stop') btn_lengths.append(self.stop_btn.GetSize()[0]) btn_total_length += btn_lengths[4] self.btn_interstice = (550 - btn_total_length) / 4 btn_pos_tmp = btn_lengths[ 0] + self.btn_interstice + 20 # 20201126: 20:init length + btn0 length + btn_inter:gap self.power_off_btn.SetPosition((btn_pos_tmp, self.btn_height)) btn_pos_tmp += btn_lengths[1] + self.btn_interstice self.reset_btn.SetPosition((btn_pos_tmp, self.btn_height)) btn_pos_tmp += btn_lengths[2] + self.btn_interstice self.home_btn.SetPosition((btn_pos_tmp, self.btn_height)) btn_pos_tmp += btn_lengths[3] + self.btn_interstice self.stop_btn.SetPosition((btn_pos_tmp, self.btn_height)) self.servo_state_label = wx.StaticText(self.panel, label='Servo state:', pos=(590, self.btn_height - 10)) self.servo_state_show = wx.TextCtrl(self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value='', pos=(600, self.btn_height + 10)) self.servo_state = bool() self.servo_state_lock = threading.Lock() self.fault_state_label = wx.StaticText(self.panel, label='Fault state:', pos=(590, self.btn_height + 60)) self.fault_state_show = wx.TextCtrl(self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value='', pos=(600, self.btn_height + 80)) self.fault_state = bool() self.fault_state_lock = threading.Lock() # 20201209: add the description of end button self.end_button_state_label = wx.StaticText(self.panel, label='END Button\n state', pos=(555, self.btn_height + 160)) self.reply_show_label = wx.StaticText( self.panel, label='Result:', pos=(20, self.btn_height + 260)) # 20201126: btn_height from 120 change to 260. self.reply_show = wx.TextCtrl( self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value='', size=(670, 30), pos=(20, self.btn_height + 280)) # 20201126: btn_height from 140 change to 280. link_textctrl_length = (btn_pos_tmp - 40) / 2 self.ref_links_show_label = wx.StaticText( self.panel, label='Ref. link:', pos=(20, self.btn_height + 210)) # 20201126: btn_height from 60 change to 210. self.ref_link_show = wx.TextCtrl( self.panel, style=(wx.TE_READONLY), value=self.ref_link_name, size=(link_textctrl_length, 30), pos=(20, self.btn_height + 230)) # 20201126: btn_height from 80 change to 230. self.end_link_show_label = wx.StaticText( self.panel, label='End link:', pos=(link_textctrl_length + 30, self.btn_height + 210)) # 20201126: btn_height from 80 change to 200. self.end_link_show = wx.TextCtrl(self.panel, style=(wx.TE_READONLY), value=self.end_link_name, size=(link_textctrl_length, 30), pos=(link_textctrl_length + 30, self.btn_height + 230)) self.set_links_btn = wx.Button(self.panel, label='Set links', name='Set links') self.set_links_btn.SetPosition( (btn_pos_tmp, self.btn_height + 230)) # 20201126: btn_height from 75 change to 220. # the variables about velocity scaling velocity_scaling_init = rospy.get_param(self.elfin_basic_api_ns + 'velocity_scaling', default=0.1) default_velocity_scaling = str(round(velocity_scaling_init, 2)) self.velocity_setting_label = wx.StaticText( self.panel, label='Velocity Scaling', pos=(20, self.btn_height - 55)) # 20201126: btn_height from 70 change to 55 self.velocity_setting = wx.Slider( self.panel, value=int(velocity_scaling_init * 100), minValue=1, maxValue=100, style=wx.SL_HORIZONTAL, size=(500, 30), pos=(45, self.btn_height - 35)) # 20201126: btn_height from 70 change to 35 self.velocity_setting_txt_lower = wx.StaticText( self.panel, label='1%', pos=(20, self.btn_height - 35)) # 20201126: btn_height from 45 change to 35 self.velocity_setting_txt_upper = wx.StaticText( self.panel, label='100%', pos=(550, self.btn_height - 35)) # 20201126: btn_height from 45 change to 35 self.velocity_setting_show = wx.TextCtrl( self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value=default_velocity_scaling, pos=(600, self.btn_height - 45)) # 20201126: btn_height from 55 change to 45 self.velocity_setting.Bind(wx.EVT_SLIDER, self.velocity_setting_cb) self.teleop_api_dynamic_reconfig_client = dynamic_reconfigure.client.Client( self.elfin_basic_api_ns, config_callback=self.basic_api_reconfigure_cb) self.dlg = wx.Dialog(self.panel, title='messag') self.dlg.Bind(wx.EVT_CLOSE, self.closewindow) self.dlg_panel = wx.Panel(self.dlg) self.dlg_label = wx.StaticText(self.dlg_panel, label='hello', pos=(15, 15)) self.set_links_dlg = wx.Dialog(self.panel, title='Set links', size=(400, 100)) self.set_links_dlg_panel = wx.Panel(self.set_links_dlg) self.sld_ref_link_show = wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER, value='', pos=(20, 20), size=(link_textctrl_length, 30)) self.sld_end_link_show = wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER, value='', pos=(20, 70), size=(link_textctrl_length, 30)) self.sld_set_ref_link_btn = wx.Button(self.set_links_dlg_panel, label='Update ref. link', name='Update ref. link') self.sld_set_ref_link_btn.SetPosition((link_textctrl_length + 30, 15)) self.sld_set_end_link_btn = wx.Button(self.set_links_dlg_panel, label='Update end link', name='Update end link') self.sld_set_end_link_btn.SetPosition((link_textctrl_length + 30, 65)) self.set_links_dlg.SetSize( (link_textctrl_length + self.sld_set_ref_link_btn.GetSize()[0] + 50, 120)) self.call_teleop_joint = rospy.ServiceProxy( self.elfin_basic_api_ns + 'joint_teleop', SetInt16) self.call_teleop_joint_req = SetInt16Request() self.call_teleop_cart = rospy.ServiceProxy( self.elfin_basic_api_ns + 'cart_teleop', SetInt16) self.call_teleop_cart_req = SetInt16Request() self.call_teleop_stop = rospy.ServiceProxy( self.elfin_basic_api_ns + 'stop_teleop', SetBool) self.call_teleop_stop_req = SetBoolRequest() self.call_stop = rospy.ServiceProxy( self.elfin_basic_api_ns + 'stop_teleop', SetBool) self.call_stop_req = SetBoolRequest() self.call_stop_req.data = True self.stop_btn.Bind( wx.EVT_BUTTON, lambda evt, cl=self.call_stop, rq=self.call_stop_req: self. call_set_bool_common(evt, cl, rq)) self.call_reset = rospy.ServiceProxy( self.elfin_driver_ns + 'clear_fault', SetBool) self.call_reset_req = SetBoolRequest() self.call_reset_req.data = True self.reset_btn.Bind( wx.EVT_BUTTON, lambda evt, cl=self.call_reset, rq=self.call_reset_req: self. call_set_bool_common(evt, cl, rq)) self.call_power_on = rospy.ServiceProxy( self.elfin_basic_api_ns + 'enable_robot', SetBool) self.call_power_on_req = SetBoolRequest() self.call_power_on_req.data = True self.power_on_btn.Bind( wx.EVT_BUTTON, lambda evt, cl=self.call_power_on, rq=self.call_power_on_req: self. call_set_bool_common(evt, cl, rq)) self.call_power_off = rospy.ServiceProxy( self.elfin_basic_api_ns + 'disable_robot', SetBool) self.call_power_off_req = SetBoolRequest() self.call_power_off_req.data = True self.power_off_btn.Bind( wx.EVT_BUTTON, lambda evt, cl=self.call_power_off, rq=self.call_power_off_req: self.call_set_bool_common(evt, cl, rq)) self.call_move_homing = rospy.ServiceProxy( self.elfin_basic_api_ns + 'home_teleop', SetBool) self.call_move_homing_req = SetBoolRequest() self.call_move_homing_req.data = True self.home_btn.Bind( wx.EVT_LEFT_DOWN, lambda evt, cl=self.call_move_homing, rq=self.call_move_homing_req: self.call_set_bool_common(evt, cl, rq)) self.home_btn.Bind( wx.EVT_LEFT_UP, lambda evt, mark=100: self.release_button(evt, mark)) self.call_set_ref_link = rospy.ServiceProxy( self.elfin_basic_api_ns + 'set_reference_link', SetString) self.call_set_end_link = rospy.ServiceProxy( self.elfin_basic_api_ns + 'set_end_link', SetString) self.set_links_btn.Bind(wx.EVT_BUTTON, self.show_set_links_dialog) self.sld_set_ref_link_btn.Bind(wx.EVT_BUTTON, self.update_ref_link) self.sld_set_end_link_btn.Bind(wx.EVT_BUTTON, self.update_end_link) self.sld_ref_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_ref_link) self.sld_end_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_end_link) self.action_client = SimpleActionClient( self.controller_ns + 'follow_joint_trajectory', FollowJointTrajectoryAction) self.action_goal = FollowJointTrajectoryGoal() self.action_goal.trajectory.joint_names = self.joint_names self.SetMinSize(the_size) self.SetMaxSize(the_size) def display_init(self): js_pos = [20, 20] js_btn_length = [70, 70, 61, 80] js_distances = [10, 20, 10, 26] dis_h = 50 for i in xrange(len(self.js_display)): self.jp_button[i] = wx.Button(self.panel, label='J' + str(i + 1) + ' +', pos=(js_pos[0], js_pos[1] + (5 - i) * dis_h), size=(70, 40)) dis_tmp = js_btn_length[0] + js_distances[0] self.jp_button[i].Bind( wx.EVT_LEFT_DOWN, lambda evt, mark=i + 1: self.teleop_joints(evt, mark)) self.jp_button[i].Bind( wx.EVT_LEFT_UP, lambda evt, mark=i + 1: self.release_button(evt, mark)) self.jm_button[i] = wx.Button(self.panel, label='J' + str(i + 1) + ' -', pos=(js_pos[0] + dis_tmp, js_pos[1] + (5 - i) * dis_h), size=(70, 40)) dis_tmp += js_btn_length[1] + js_distances[1] self.jm_button[i].Bind(wx.EVT_LEFT_DOWN, lambda evt, mark=-1 * (i + 1): self.teleop_joints(evt, mark)) self.jm_button[i].Bind(wx.EVT_LEFT_UP, lambda evt, mark=-1 * (i + 1): self.release_button(evt, mark)) pos_js_label = (js_pos[0] + dis_tmp, js_pos[1] + (5 - i) * dis_h) self.js_label[i] = wx.StaticText(self.panel, label='J' + str(i + 1) + '/deg:', pos=pos_js_label) self.js_label[i].SetPosition( (pos_js_label[0], pos_js_label[1] + abs(40 - self.js_label[i].GetSize()[1]) / 2)) dis_tmp += js_btn_length[2] + js_distances[2] pos_js_display = (js_pos[0] + dis_tmp, js_pos[1] + (5 - i) * dis_h) self.js_display[i] = wx.TextCtrl(self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value=' 0000.00 ', pos=pos_js_display) self.js_display[i].SetPosition( (pos_js_display[0], pos_js_display[1] + abs(40 - self.js_display[i].GetSize()[1]) / 2)) dis_tmp += js_btn_length[3] + js_distances[3] ps_pos = [js_pos[0] + dis_tmp, 20] ps_btn_length = [70, 70, 53, 80] ps_distances = [10, 20, 10, 20] pcs_btn_label = ['X', 'Y', 'Z', 'Rx', 'Ry', 'Rz'] pcs_label = ['X', 'Y', 'Z', 'R', 'P', 'Y'] unit_label = ['/mm:', '/mm:', '/mm:', '/deg:', '/deg:', '/deg:'] for i in xrange(len(self.ps_display)): self.pp_button[i] = wx.Button(self.panel, label=pcs_btn_label[i] + ' +', pos=(ps_pos[0], ps_pos[1] + (5 - i) * dis_h), size=(70, 40)) dis_tmp = ps_btn_length[0] + ps_distances[0] self.pp_button[i].Bind( wx.EVT_LEFT_DOWN, lambda evt, mark=i + 1: self.teleop_pcs(evt, mark)) self.pp_button[i].Bind( wx.EVT_LEFT_UP, lambda evt, mark=i + 1: self.release_button(evt, mark)) self.pm_button[i] = wx.Button(self.panel, label=pcs_btn_label[i] + ' -', pos=(ps_pos[0] + dis_tmp, ps_pos[1] + (5 - i) * dis_h), size=(70, 40)) dis_tmp += ps_btn_length[1] + ps_distances[1] self.pm_button[i].Bind(wx.EVT_LEFT_DOWN, lambda evt, mark=-1 * (i + 1): self.teleop_pcs(evt, mark)) self.pm_button[i].Bind(wx.EVT_LEFT_UP, lambda evt, mark=-1 * (i + 1): self.release_button(evt, mark)) pos_ps_label = (ps_pos[0] + dis_tmp, ps_pos[1] + (5 - i) * dis_h) self.ps_label[i] = wx.StaticText(self.panel, label=pcs_label[i] + unit_label[i], pos=pos_ps_label) self.ps_label[i].SetPosition( (pos_ps_label[0], pos_ps_label[1] + abs(40 - self.ps_label[i].GetSize()[1]) / 2)) dis_tmp += ps_btn_length[2] + ps_distances[2] pos_ps_display = (ps_pos[0] + dis_tmp, ps_pos[1] + (5 - i) * dis_h) self.ps_display[i] = wx.TextCtrl(self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value='', pos=pos_ps_display) self.ps_display[i].SetPosition( (pos_ps_display[0], pos_ps_display[1] + abs(40 - self.ps_display[i].GetSize()[1]) / 2)) dis_tmp += ps_btn_length[3] + ps_distances[3] # 20201209: add the DO,LED,DI,end button. for i in xrange(len(self.DO_btn_display)): self.DO_btn_display[i] = wx.Button( self.panel, label='DO' + str(i), pos=(20 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i, self.btn_height + 40)) self.DO_btn_display[i].Bind( wx.EVT_BUTTON, lambda evt, marker=i, cl=self.call_write_DO: self. call_write_DO_command(evt, marker, cl)) self.DI_display[i] = wx.TextCtrl( self.panel, style=(wx.TE_CENTER | wx.TE_READONLY), value='DI' + str(i), size=(self.DO_btn_display[i].GetSize()), pos=(20 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i, self.btn_height + 80)) self.LED_display[i] = wx.Button( self.panel, label='LED' + str(i), pos=(20 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i, self.btn_height + 120)) self.LED_display[i].Bind( wx.EVT_BUTTON, lambda evt, marker=4 + i, cl=self.call_write_DO: self. call_write_DO_command(evt, marker, cl)) png = wx.Image( self.btn_path + '/btn_icon/End_btn' + str(i) + '_low.png', wx.BITMAP_TYPE_PNG).ConvertToBitmap() self.End_btn_display[i] = wx.StaticBitmap( self.panel, -1, png, pos=(40 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i, self.btn_height + 160)) def velocity_setting_cb(self, event): current_velocity_scaling = self.velocity_setting.GetValue() * 0.01 self.teleop_api_dynamic_reconfig_client.update_configuration( {'velocity_scaling': current_velocity_scaling}) wx.CallAfter(self.update_velocity_scaling_show, current_velocity_scaling) def basic_api_reconfigure_cb(self, config): if self.velocity_setting_show.GetValue() != config.velocity_scaling: self.velocity_setting.SetValue(int(config.velocity_scaling * 100)) wx.CallAfter(self.update_velocity_scaling_show, config.velocity_scaling) def action_stop(self): self.action_client.wait_for_server(timeout=rospy.Duration(secs=0.5)) self.action_goal.trajectory.header.stamp.secs = 0 self.action_goal.trajectory.header.stamp.nsecs = 0 self.action_goal.trajectory.points = [] self.action_client.send_goal(self.action_goal) def teleop_joints(self, event, mark): self.call_teleop_joint_req.data = mark resp = self.call_teleop_joint.call(self.call_teleop_joint_req) wx.CallAfter(self.update_reply_show, resp) event.Skip() def teleop_pcs(self, event, mark): self.call_teleop_cart_req.data = mark resp = self.call_teleop_cart.call(self.call_teleop_cart_req) wx.CallAfter(self.update_reply_show, resp) event.Skip() def release_button(self, event, mark): self.call_teleop_stop_req.data = True resp = self.call_teleop_stop.call(self.call_teleop_stop_req) wx.CallAfter(self.update_reply_show, resp) event.Skip() def call_set_bool_common(self, event, client, request): btn = event.GetEventObject() check_list = ['Servo On', 'Servo Off', 'Clear Fault'] # Check servo state if btn.GetName() == 'Servo On': servo_enabled = bool() if self.servo_state_lock.acquire(): servo_enabled = self.servo_state self.servo_state_lock.release() if servo_enabled: resp = SetBoolResponse() resp.success = False resp.message = 'Robot is already enabled' wx.CallAfter(self.update_reply_show, resp) event.Skip() return # Check fault state if btn.GetName() == 'Clear Fault': fault_flag = bool() if self.fault_state_lock.acquire(): fault_flag = self.fault_state self.fault_state_lock.release() if not fault_flag: resp = SetBoolResponse() resp.success = False resp.message = 'There is no fault now' wx.CallAfter(self.update_reply_show, resp) event.Skip() return # Check if the button is in check list if btn.GetName() in check_list: self.show_message_dialog(btn.GetName(), client, request) else: try: resp = client.call(request) wx.CallAfter(self.update_reply_show, resp) except rospy.ServiceException, e: resp = SetBoolResponse() resp.success = False resp.message = 'no such service in simulation' wx.CallAfter(self.update_reply_show, resp) event.Skip()
class MoveBaseClient: global_tf = None def __init__(self, record_rate=5, timeout=90): if MoveBaseClient.global_tf is None: MoveBaseClient.global_tf = tf.TransformListener() self.tf = MoveBaseClient.global_tf self.ac = SimpleActionClient('move_base', MoveBaseAction) self.record_rate = record_rate self.base_frame = '/map' self.target_frame = '/base_footprint' self.timeout = rospy.Duration(timeout) self.recording = False self.other_data = [] self.subscriptions = [] self.subscribers = [] printed = False limit = rospy.Time.now() + rospy.Duration(30) while not self.ac.wait_for_server(rospy.Duration( 5.0)) and not rospy.is_shutdown() and rospy.Time.now() < limit: printed = True rospy.loginfo('Waiting for move_base server') if rospy.Time.now() >= limit: self.ac = None return if rospy.is_shutdown(): self.ac = None return if printed: rospy.loginfo('Got move_base server') def ready(self): return self.ac is not None def addSubscription(self, topic, msg_type): self.subscriptions.append((topic, msg_type)) def reset(self): for name, proxy in self.resets.iteritems(): proxy() rospy.loginfo("Reset called on %s" % name) def cb(self, msg, topic): if not self.recording: return if topic == '/move_base_node/DWAPlannerROS/local_plan': msg.header.frame_id = '/map' for i, pose in enumerate(msg.poses): msg.poses[i] = self.tf.transformPose('/map', pose) elif topic == '/move_base_node/local_costmap/costmap': p = PoseStamped() p.header = msg.header p.pose = msg.info.origin np = self.tf.transformPose('/map', p) msg.header = np.header msg.info.origin = np.pose self.other_data.append((rospy.Time.now(), topic, msg)) def goto(self, loc, debug=False): if self.ac is None: return [] self.goal = loc q = quaternion_from_euler(0, 0, loc[2]) goal = MoveBaseGoal() goal.target_pose.header.frame_id = self.base_frame goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = loc[0] goal.target_pose.pose.position.y = loc[1] goal.target_pose.pose.orientation.w = q[3] goal.target_pose.pose.orientation.x = q[0] goal.target_pose.pose.orientation.y = q[1] goal.target_pose.pose.orientation.z = q[2] rate = rospy.Rate(self.record_rate) self.data = [] self.other_data = [] self.recording = True self.subscribers = [] for topic, msg_type in self.subscriptions: sub = rospy.Subscriber(topic, msg_type, self.cb, topic) self.subscribers.append(sub) rospy.sleep(0.5) self.ac.send_goal(goal) if debug: timer = rospy.Timer(rospy.Duration(5), self.print_distance) start_time = rospy.Time.now() while not finished(self.ac.get_state() ) and rospy.Time.now() < start_time + self.timeout: t, pose = get_time_and_pose(self.tf, self.base_frame, self.target_frame) if pose is not None: self.data.append((t, "/robot_pose", pose)) else: print 'TF Error' rate.sleep() if debug: timer.shutdown() self.recording = False rospy.sleep(1) for sub in self.subscribers: sub.unregister() # save footprint footprint_param = rospy.get_param('/move_base_node/footprint', []) footprint = Polygon() if type(footprint_param) == type([]): for x, y in footprint_param: footprint.points.append(Point32(x, y, 0.0)) else: #string for subs in footprint_param[2:-2].split('],['): x, y = subs.split(',') footprint.points.append(Point32(float(x), float(y), 0.0)) self.other_data.append((t, '/footprint', footprint)) # save the rest of the configuration params = rospy.get_param('/') params_str = std_msgs.msg.String() params_str.data = yaml.dump(params, default_flow_style=False) self.other_data.append((t, '/parameters', params_str)) return self.data + self.other_data def print_distance(self, event=None): if len(self.data) == 0: return pose = self.data[-1][2] dx = abs(self.goal[0] - pose.x) dy = abs(self.goal[1] - pose.y) dt = abs(self.goal[2] - pose.theta) rospy.loginfo("dx: %.2f dy: %.2f dt: %d" % (dx, dy, int(dt * 180 / 3.141))) def load_subscriptions(self): #TODO: Load classes dynamically topics = rospy.get_param('/nav_experiments/topics', []) for topic in topics: if 'plan' in topic: self.addSubscription(topic, Path) elif 'command' in topic or 'cmd_vel' in topic: self.addSubscription(topic, Twist) elif 'costmap_updates' in topic: self.addSubscription(topic, OccupancyGridUpdate) elif 'costmap/costmap' in topic: self.addSubscription(topic, OccupancyGrid) elif 'cloud' in topic: self.addSubscription(topic, PointCloud2) else: rospy.logerr("unknown type for %s" % topic) self.addSubscription('/collisions', std_msgs.msg.String) self.addSubscription('/simulation_state', ModelStates) self.addSubscription('/move_base_node/global_costmap/update_time', std_msgs.msg.Float32) self.addSubscription('/move_base_node/local_costmap/update_time', std_msgs.msg.Float32) self.addSubscription('/people', people_msgs.msg.People)
class Pick_Place: def __init__(self): self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(1.0) self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._grasps_ac = SimpleActionClient( '/autoarm_grasp_pose_server/generate_grasp_poses', GenerateGraspPosesAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.45 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose def _add_grasp_block_(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.03, 0.03, 0.09)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspPosesGoal() goal.pose = pose goal.width = width options = GraspPoseOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspPoseOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspPoseOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
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 Driver(): HOME = 0 RANDOMIZE = 1 NORMAL = 2 SHUTDOWN = 3 def __init__(self, home_x=2.0, home_y=2.0): # flag to check for termination conditions self.is_running = True self.condition = Driver.NORMAL # get positions from file rospack = rospkg.RosPack() dir_path = rospack.get_path("exercise1") file_path = dir_path + "/extra/positions.csv" # positions = [(2.0, 2.0), (3.0, 3.0), (5.0, 3.0)] self.orig_positions = np.loadtxt(open(file_path, "rb"), delimiter=",") print "Locations to visit - pose(x,y,z) quaternion(x,y,z,w) " print self.orig_positions # self.home_x = home_x # self.home_y = home_y self.start_loc = (2.0, 2.0, 0.0, 0.0, 0.0, 0.988, 0.156) # to_visit = list(reversed(orig_positions)) self.to_visit = copy.deepcopy(self.orig_positions) self.wait_time = 60.0 # used as seconds in autopilot function self.is_interrupted = False # create the action client and send goal self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server(rospy.Duration.from_sec(self.wait_time)) ## marking list of places to vist self.location_marker_pub = rospy.Publisher("destination_marker", Marker, queue_size=10) self.create_all_markers() # publishing destination thread self.marker_pub_thread = MarkerPublisherThread( "marker_pub_thread", 1.0, self.location_marker_pub, self.markers) # keyboard input thread self.kb_thread = KeyboardWorkerThread("kb_thread", .05) # important to register shutdown node with threads after thread have been created rospy.on_shutdown(self.shutdown) # start threads before handing off state control to autopilot self.marker_pub_thread.start() self.kb_thread.start() # rospy.on_shutdown(self.shutdown_node) def autopilot(self): ''' visit each location in visit list ''' while (True): # print "Locations to visit" # print self.to_visit for loc in self.to_visit: # print # print # print "Location to visit" # print loc self.send_move_goal(loc) # self.client.wait_for_result(rospy.Duration.from_sec(self.wait_time)) # blocking call # could check to make sure its not a failure nor a success terminal_states = [ GoalStatus.REJECTED, GoalStatus.ABORTED, GoalStatus.SUCCEEDED, GoalStatus.RECALLED ] # print "Terminal States: ", terminal_states movement_state = self.client.get_state() # print "Condition!!", self.condition # print terminal_states while movement_state not in terminal_states: movement_state = self.client.get_state() if self.update_state() != Driver.NORMAL: break time.sleep(.1) if movement_state == GoalStatus.SUCCEEDED: self.do_something_interesting() if self.condition != Driver.NORMAL: # print "Condition not normal!!" break if self.condition == Driver.HOME: ## Go home and start visiting list again espeak_call_str = "espeak -s 110 \'Yes master. Returning home.\'" os.system(espeak_call_str) self.send_move_goal(self.start_loc) print "Visiting Home" ## wait till you get home to start the next round self.client.wait_for_result( rospy.Duration.from_sec(self.wait_time)) self.do_something_interesting() elif self.condition == Driver.RANDOMIZE: ## jumble the to-visit list then start visiting list again np.random.shuffle(self.to_visit) # print "Shuffled List" elif self.condition == Driver.SHUTDOWN: print "Shutting down" espeak_call_str = "espeak -s 110 \'Goodbye.\'" os.system(espeak_call_str) break self.condition = Driver.NORMAL # everything goes to normal once weird stuff happens # print "Visiting locations again" time.sleep(2) # print "exiting autopilot" # rospy.signal_shutdown("Finished Autopilot") def update_state(self): char = self.kb_thread.pop_c() if char == 'h': self.condition = Driver.HOME elif char == 'r': self.condition = Driver.RANDOMIZE elif char == '\x1b': self.condition = Driver.SHUTDOWN # print "shutdown condition" else: # self.condition = Driver.NORMAL pass return self.condition def cancel_goal(self): self.client.cancel_all_goals() def send_move_goal(self, loc): # create goal goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = loc[0] goal.target_pose.pose.position.y = loc[1] goal.target_pose.pose.position.z = loc[2] goal.target_pose.pose.orientation.x = loc[3] goal.target_pose.pose.orientation.y = loc[4] goal.target_pose.pose.orientation.z = loc[5] goal.target_pose.pose.orientation.w = loc[6] goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) return self.client def do_something_interesting(self): ''' Output a nice call phrase for the grader. Assumes espeak is installed. BAD ASSUMPTION ''' sayings = [ "I love you Will", "Great job Will", "Your a great teaching assistant", "Am I home yet?", "When will this stop?", "Are you my mother?", "Assimov books are great.", "Robot army on its way.", "Your my friend.", "Kory deserves 100 percent." ] espeak_call_str = "espeak -s 120 \'{0}\'".format(choice(sayings)) os.system(espeak_call_str) def create_all_markers(self): self.markers = [] for i in xrange(len(self.orig_positions)): loc = self.orig_positions[i] marker_id = i marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0, 0, 1, 1)) self.markers.append(marker) # print "in create all markers", self.markers def mark_as_to_visit(self, loc, marker_id): marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0, 1, 0, 1)) self.location_marker_pub.publish(marker) def mark_as_visited(self, loc, marker_id): marker = self.create_location_marker(loc, color, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0, 1, 1, 1)) self.location_marker_pub.publish(marker) def create_location_marker(self, loc, marker_type=Marker.SPHERE, marker_action=Marker.ADD, marker_id=0, marker_color=ColorRGBA(1, 0, 0, 1)): h = Header() h.frame_id = "map" h.stamp = rospy.Time.now() mark = Marker() mark.header = h mark.ns = "location_marker" mark.id = marker_id mark.type = marker_type mark.action = marker_action mark.scale = Vector3(0.25, 0.25, 0.25) mark.color = marker_color pose = Pose(Point(loc[0], loc[1], loc[2]), Quaternion(loc[3], loc[4], loc[5], loc[6])) mark.pose = pose return mark ### threaded way to do it def shutdown(self): self.client.cancel_all_goals() # self.marker_pub_thread.quit() self.marker_pub_thread.terminate() old_settings = self.kb_thread.old_settings self.kb_thread.quit() self.kb_thread.terminate() # print "terminated termios and restored settings" termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
roslib.load_manifest('w2_arm_navigation_tests') import rospy from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from arm_navigation_msgs.msg import MoveArmAction from arm_navigation_msgs.msg import MoveArmGoal from arm_navigation_msgs.msg import JointConstraint from arm_navigation_msgs.msg import LinkPadding if __name__ == '__main__': rospy.init_node('move_arm_simple_joint_goal_node', anonymous=True) move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction) rospy.loginfo('init_node complete') 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) links = ('L7_wrist_roll_link', 'L6_wrist_pitch_link', 'L5_forearm_roll_link', 'L4_elbow_flex_link', 'L3_upperarm_roll_link', 'L2_shoulder_pan_link', 'L1_shoulder_pitch_link', 'L8_left_finger_link ', 'L9_right_finger_link')
class Pick_Place: def __init__(self): # 检索参数: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._table2_object_name = rospy.get_param('~table_object_name', 'Grasp_Table2') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object2') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # 创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # 创建规划现场,机器人指挥官: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # 清理现场: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) # 将表和可乐罐对象添加到计划场景: self._pose_table = self._add_table(self._table_object_name) self._pose_table = self._add_table2(self._table2_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) self._pose_coke_can2 = self._add_grasp_block2_(self._grasp_object2_name) rospy.sleep(1.0) # 定义目标位置姿势: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x-0.5 self._pose_place.position.y = self._pose_coke_can.position.y-0.85 self._pose_place.position.z = self._pose_coke_can.position.z-0.3 self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # 检索组 (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('pickup') self._arm.go(wait=True) print("Pickup pose") # 创建抓取生成器“生成”动作客户端: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # 创建移动组“zhua取”操作客户端: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # 创建移动组“放置”动作客户端: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # 放置可乐可能会在支撑表面(桌子)上的其他地方产生异物: while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') print "-------------test1--------------" rospy.sleep(1.0) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) print "-------------test2------------------" def _add_table(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.85 p.pose.position.y = 0.0 p.pose.position.z = 0.70 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (1, 1, 0.05)) print "-------------test3------------------" return p.pose def _add_table2(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.85 p.pose.position.z = 0.40 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.5, 0.05)) print "-------------test4------------------" return p.pose def _add_grasp_block_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test5------------------" return p.pose def _add_grasp_block2_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.3 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test6------------------" return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例 Generate the grasp Pose Array data for visualization, and then send the grasp goal to the grasp server. 生成抓取姿势阵列数据以进行可视化,然后将抓取目标发送到抓取服务器。 """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # 发送目标并等待结果: # 将目标发送到ActionServer,等待目标完成,并且必须抢占目标. state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # 发布掌握(用于调试/可视化目的): self._publish_grasps(grasps) print "-------------test7------------------" return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # 生成预安置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) print "-------------test8------------------" return places def _create_pickup_goal(self, group, target, grasps): """ 创建一个MoveIt! 接送目标 创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # 配置目标计划选项: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test9------------------" return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test10------------------" return goal def _pickup(self, group, target, width): """ 使用计划小组选择目标 """ # 从掌握生成器服务器获取可能的掌握: grasps = self._generate_grasps(self._pose_coke_can, width) # 创建并发送提货目标: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test11------------------" return True def _place(self, group, target, place): """ 使用计划组放置目标 """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test12------------------" return True def _publish_grasps(self, grasps): """ 使用PoseArray消息将抓取发布为姿势 """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) print "-------------test13------------------" def _publish_places(self, places): """ 使用PoseArray消息将位置发布为姿势 """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) print "-------------test14------------------"
class PickAruco(object): def __init__(self): rospy.loginfo("Initalizing...") self.bridge = CvBridge() self.tfBuffer = tf2_ros.Buffer() self.tf_l = tf2_ros.TransformListener(self.tfBuffer) rospy.loginfo("Waiting for /pickup_pose AS...") self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction) time.sleep(1.0) if not self.pick_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /pickup_pose AS") exit() rospy.loginfo("Waiting for /place_pose AS...") self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction) self.place_as.wait_for_server() rospy.loginfo("Setting publishers to torso and head controller...") self.torso_cmd = rospy.Publisher( '/torso_controller/command', JointTrajectory, queue_size=1) self.head_cmd = rospy.Publisher( '/head_controller/command', JointTrajectory, queue_size=1) self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose', PoseStamped, queue_size=1, latch=True) # /arm_controller/command (trajectory_msgs/JointTrajectory) self.trajectory_cmd = rospy.Publisher( '/arm_controller/command', JointTrajectory, queue_size=1) self.gripper_cmd = rospy.Publisher( '/gripper_controller/command', JointTrajectory, queue_size=1) self.hey5_cmd = rospy.Publisher( '/hand_controller/command', JointTrajectory, queue_size=1) rospy.loginfo("Waiting for '/play_motion' AS...") self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction) if not self.play_m_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /play_motion AS") exit() rospy.loginfo("Connected!") rospy.sleep(1.0) rospy.loginfo("Done initializing PickAruco.") def strip_leading_slash(self, s): return s[1:] if s.startswith("/") else s def pick_aruco(self, string_operation): # self.prepare_robot() self.open_hey5() # self.close_hey5() # rospy.sleep(3.0) # rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection") # # self.turn_wrist() # experimental function # # # get aruco pose here: from marker # # aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped) # # aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id) # # manually input aruco pose # aruco_pose = PoseStamped() # aruco_pose.header.frame_id = 'base_footprint' # # # original aruco pose # # aruco_pose.pose.position.x = 0.528450879403 # # aruco_pose.pose.position.y = -0.0486955713869 # # aruco_pose.pose.position.z = 0.922035729832 # aruco_pose.pose.position.x = 0.544079 # aruco_pose.pose.position.y = 0.050645 #+ 0.05 # aruco_pose.pose.position.z = 0.706404 + 0.08 # aruco_pose.pose.orientation.x = 0.5 # aruco_pose.pose.orientation.y = 0.5 # aruco_pose.pose.orientation.z = 0.5 # aruco_pose.pose.orientation.w = 0.5 # rospy.loginfo("Got: " + str(aruco_pose)) # rospy.loginfo("spherical_grasp_gui: Transforming from frame: " + # aruco_pose.header.frame_id + " to 'base_footprint'") # ps = PoseStamped() # ps.pose.position = aruco_pose.pose.position # ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id) # ps.header.frame_id = aruco_pose.header.frame_id # transform_ok = False # while not transform_ok and not rospy.is_shutdown(): # try: # transform = self.tfBuffer.lookup_transform("base_footprint", # ps.header.frame_id, # rospy.Time(0)) # aruco_ps = do_transform_pose(ps, transform) # transform_ok = True # except tf2_ros.ExtrapolationException as e: # rospy.logwarn( # "Exception on transforming point... trying again \n(" + # str(e) + ")") # rospy.sleep(0.01) # ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id) # pick_g = PickUpPoseGoal() # if string_operation == "pick": # rospy.loginfo("Setting cube pose based on ArUco detection") # pick_g.object_pose.pose.position = aruco_ps.pose.position # pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) # rospy.loginfo("aruco pose in base_footprint:" + str(pick_g)) # pick_g.object_pose.header.frame_id = 'base_footprint' # pick_g.object_pose.pose.orientation.w = 1.0 # self.detected_pose_pub.publish(pick_g.object_pose) # rospy.loginfo("Gonna pick:" + str(pick_g)) # self.pick_as.send_goal_and_wait(pick_g) # rospy.loginfo("Done!") # # some_pose=arm.get_current_pose(end_effector_link).pose # # rospy.loginfo("current hand pose "+str(some_pose)) # result = self.pick_as.get_result() # if str(moveit_error_dict[result.error_code]) != "SUCCESS": # rospy.logerr("Failed to pick, not trying further") # return # # Move torso to its maximum height # self.lift_torso() # rospy.sleep(3.0) # # self.turn_wrist() # aruco_pose.pose.position.x = 0.733477 - 0.2 # aruco_pose.pose.position.y = 0.057126 + 0.07 # aruco_pose.pose.position.z += 0.2 # 0.2 is the allowance for pouring # drop_pose = deepcopy(aruco_pose) # # drop_pose.pose.position.x += 0.2 # # drop_pose.pose.position.y += 0.3 # # drop_pose.pose.position.z += 0.2 # # optimize_goal_pose(drop_pose) # """ # The following are good good_candidates for pose selection (in eular angles): # [[0, 0, 0], [0, 0, 270], [0, 180, 0], [0, 180, 180] - no, [0, 180, 270], [0, 270, 0], [0, 270, 180], [0, 270, 270]] # """ # x, y, z, w = eular_to_q(0,270,180) # # x, y, z, w = eular_to_q(0,270,270) # drop_pose.pose.orientation.x = -0.5 # drop_pose.pose.orientation.y = 0 # drop_pose.pose.orientation.z = 0 # drop_pose.pose.orientation.w = 1.2 # success = False # while success==False: # result = cartesian_move_to(drop_pose, True) # rospy.loginfo("success of trajectory: "+str(result)) # # define a goal tolerance for replanning and manipulation # x_arm, y_arm, z_arm = arm_pose() # x_aim = drop_pose.pose.position.x # y_aim = drop_pose.pose.position.y # z_aim = drop_pose.pose.position.z # goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim, z_aim) # rospy.loginfo("Deviation from target pose: "+str(goal_deviation)) # drop_pose.pose.position.x -= 0.01 # drop_pose.pose.position.y -= 0.01 # if result > 0.9 and goal_deviation < 0.1: # success = True # x, y, z = q_to_eular(-0.5, 0, 0, 1.2) # rospy.loginfo("End effector eular angles: " + str([x,y,z])) # rospy.sleep(3) # # self.open_gripper() # # pour liquid # self.turn_wrist() # # arm=MoveGroupCommander('arm') # # arm.allow_replanning(True) # # end_effector_link=arm.get_end_effector_link() # # arm.set_goal_position_tolerance(0.03) # # arm.set_goal_orientation_tolerance(0.025) # # arm.allow_replanning(True) # # reference_frame='base_footprint' # # arm.set_pose_reference_frame(reference_frame) # # arm.set_planning_time(5) # # rospy.sleep(2) # # start_pose=arm.get_current_pose(end_effector_link).pose # # rospy.loginfo("End effector start pose: " + str(start_pose)) # # x = start_pose.orientation.x # # y = start_pose.orientation.y # # z = start_pose.orientation.z # # w = start_pose.orientation.w # # x, y, z = q_to_eular(x, y, z, w) # # rospy.loginfo("End effector eular angles: " + str([x,y,z])) # # # Raise arm # # rospy.loginfo("Moving arm to a safe pose") # # pmg = PlayMotionGoal() # # pmg.motion_name = 'pick_final_pose' # # pmg.skip_planning = False # # self.play_m_as.send_goal_and_wait(pmg) # # rospy.loginfo("Raise object done.") # # # Place the object back to its position # # rospy.loginfo("Gonna place near where it was") # # pick_g.object_pose.pose.position.z += 0.05 # # pick_g.object_pose.pose.position.y += 0.2 # # self.place_as.send_goal_and_wait(pick_g) # # rospy.loginfo("Done!") def lift_torso(self): rospy.loginfo("Moving torso up") jt = JointTrajectory() jt.joint_names = ['torso_lift_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0.34] jtp.time_from_start = rospy.Duration(2.5) jt.points.append(jtp) self.torso_cmd.publish(jt) def turn_wrist(self): wrist_state = -2.0 rospy.loginfo("Turning Arm") joint_state = rospy.wait_for_message('/joint_states', JointState) j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7] rospy.loginfo("Previous wrist state: "+str(j7)) max_wrist_state = j7 + 2.0 while j7 < max_wrist_state: jt = JointTrajectory() jt.joint_names = ['arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint'] jtp = JointTrajectoryPoint() jtp.positions = [j1, j2, j3, j4, j5, j6, j7+0.5] jtp.time_from_start = rospy.Duration(2) jt.points.append(jtp) self.trajectory_cmd.publish(jt) rospy.sleep(0.25) # check the wrist joint state joint_state = rospy.wait_for_message('/joint_states', JointState) j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7] # # jtp = JointTrajectoryPoint() # # jtp.positions = [1.5,0.0,0.0,0.0,0.0,0.0,0.0] # # jtp.time_from_start = rospy.Duration(6) # # jt.points.append(jtp) # # jtp = JointTrajectoryPoint() # # jtp.positions = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] # # jtp.time_from_start = rospy.Duration(9) # # jt.points.append(jtp) # self.trajectory_cmd.publish(jt) # rospy.loginfo("Done 1") # rospy.sleep(0.1) # jt = JointTrajectory() # jt.joint_names = ['arm_1_joint', # 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', # 'arm_5_joint', 'arm_6_joint', 'arm_7_joint'] # jtp = JointTrajectoryPoint() # jtp.positions = [3,0.0,0.0,0.0,0.0,0.0,0.0] # jtp.time_from_start = rospy.Duration(6) # jt.points.append(jtp) # self.trajectory_cmd.publish(jt) # rospy.loginfo("Done 2") # rospy.sleep(0.1) # jt = JointTrajectory() # jt.joint_names = ['arm_1_joint', # 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', # 'arm_5_joint', 'arm_6_joint', 'arm_7_joint'] # jtp = JointTrajectoryPoint() # jtp.positions = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] # jtp.time_from_start = rospy.Duration(9) # jt.points.append(jtp) # self.trajectory_cmd.publish(jt) # rospy.loginfo("Done 3") # # rospy.sleep(2.0) rospy.loginfo("Done.") def lower_head(self): rospy.loginfo("Moving head down") jt = JointTrajectory() jt.joint_names = ['head_1_joint', 'head_2_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0.0, -0.75] jtp.time_from_start = rospy.Duration(2.0) jt.points.append(jtp) self.head_cmd.publish(jt) rospy.loginfo("Done.") def prepare_robot(self): rospy.loginfo("Unfold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'arm_raise' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done.") self.lower_head() rospy.loginfo("Robot prepared.") def open_gripper(self): rospy.loginfo("Opening Gripper") jt = JointTrajectory() jt.joint_names = ['gripper_left_finger_joint', 'gripper_right_finger_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0.044, 0.044] jtp.time_from_start = rospy.Duration(0.5) jt.points.append(jtp) self.gripper_cmd.publish(jt) rospy.loginfo("Done.") def open_hey5(self): rospy.loginfo("Opening Gripper") jt = JointTrajectory() jt.joint_names = ['hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint'] jtp = JointTrajectoryPoint() jtp.positions = [-1.0, -1.0, -1.0] jtp.time_from_start = rospy.Duration(0.1) jt.points.append(jtp) jtp = JointTrajectoryPoint() jtp.positions = [0.0, 0.0, 0.0] jtp.time_from_start = rospy.Duration(2.5) jt.points.append(jtp) self.hey5_cmd.publish(jt) rospy.loginfo("Done.") def close_hey5(self): rospy.loginfo("Closing Gripper") jt = JointTrajectory() jt.joint_names = ['hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint'] jtp = JointTrajectoryPoint() jtp.positions = [2.37, 0.0, 0.0] jtp.time_from_start = rospy.Duration(0.1) jt.points.append(jtp) jtp = JointTrajectoryPoint() jtp.positions = [6.2, 6.8, 9.2] jtp.time_from_start = rospy.Duration(2.5) jt.points.append(jtp) self.hey5_cmd.publish(jt) rospy.loginfo("Done.")
#!/usr/bin/env python import roslib; roslib.load_manifest('wubble2_robot') import rospy from actionlib import SimpleActionClient from object_manipulation_msgs.msg import GraspHandPostureExecutionAction from object_manipulation_msgs.msg import GraspHandPostureExecutionGoal if __name__ == '__main__': rospy.init_node('test_gripper_posture_controller', anonymous=True) posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction) posture_controller.wait_for_server() rospy.loginfo('got gripper_posture_controller') pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE posture_controller.send_goal(pg) posture_controller.wait_for_result() rospy.loginfo('finished releasing') pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP posture_controller.send_goal(pg) posture_controller.wait_for_result() rospy.loginfo('finished grasping')
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
class StateClient(object): def __init__(self, do_register=True, silent=True): self._name = rospy.get_name() self._silent = silent self._acknowledge_publisher = Publisher('/robot/state/server', RobotModeMsg, latch=True, queue_size=5) self._state_subscriber = Subscriber('/robot/state/clients', RobotModeMsg, self.server_state_information, queue_size=10) self._state_info = ServiceProxy('/robot/state/info', GetStateInfo) self._state_changer = ActionClient('/robot/state/change', RobotModeAction) if do_register: self.client_register() def client_register(self): rospy.wait_for_service('/robot/state/register') self._register_service_client = rospy.ServiceProxy( '/robot/state/register', RegisterNodeSrv) req = RegisterNodeSrvRequest() req.nodeName = self._name req.type = RegisterNodeSrvRequest.TYPE_STARTED try: self._register_service_client(req) except rospy.ServiceException: logerr("[%s] Failed to register node. Retrying...", self._name) def client_initialize(self): rospy.wait_for_service('/robot/state/register') self._register_service_client = rospy.ServiceProxy( '/robot/state/register', RegisterNodeSrv) req = RegisterNodeSrvRequest() req.nodeName = self._name req.type = RegisterNodeSrvRequest.TYPE_INITIALIZED try: self._register_service_client(req) except rospy.ServiceException: logerr("[%s] Failed to register node. Retrying...", self._name) def start_transition(self, state): if not self._silent: loginfo("[%s] Starting Transition to state %i", self._name, state) self.transition_complete(state) def transition_complete(self, state): if not self._silent: loginfo("[%s] Node Transition to state %i Completed", self._name, state) msg = RobotModeMsg() msg.nodeName = self._name msg.mode = state msg.type = RobotModeMsg.TYPE_ACK self._acknowledge_publisher.publish(msg) def get_current_state(self): """ Returns the current robot state from the state manager. """ req = GetStateInfoRequest() req.option = GetStateInfoRequest.CURRENT_STATE try: res = self._state_info(req) if res.state == -1: # There is a bug with the server. logerr('GetStateInfo: %s is not a valid request.', req.option) return res.state except rospy.ServiceException: logerr('Service GetStateInfo failed to respond.') return None def get_previous_state(self): """ Returns the previous robot state from the state manager. """ req = GetStateInfoRequest() req.option = GetStateInfoRequest.PREVIOUS_STATE try: res = self._state_info(req) if res.state == -1: # There is a bug with the server. logerr('GetStateInfo: %s is not a valid request.', req.option) return res.state except rospy.ServiceException: logerr('Service GetStateInfo failed to respond.') return None def transition_to_state(self, state): if not self._silent: loginfo("[%s] Requesting transition to state %i", self._name, state) msg = RobotModeMsg() msg.nodeName = self._name msg.mode = state msg.type = RobotModeMsg.TYPE_REQUEST self._acknowledge_publisher.publish(msg) def change_state_and_wait(self, state, timeout=None): """ Changes the state and waits for all the clients to change. :param :state A state to transition to. :param :timeout A timeout in seconds to complete the state transition. """ timeout = rospy.Duration(timeout) if timeout else rospy.Duration() msg = RobotModeMsg() msg.mode = state msg.nodeName = self._name msg.type = RobotModeMsg.TYPE_REQUEST goal = RobotModeGoal(modeMsg=msg) self._state_changer.wait_for_server() self._state_changer.send_goal(goal) return self._state_changer.wait_for_result(timeout=timeout) def server_state_information(self, msg): if not self._silent: loginfo("[%s] Received new information from state server", self._name) if msg.type == RobotModeMsg.TYPE_TRANSITION: self.start_transition(msg.mode) elif msg.type == RobotModeMsg.TYPE_START: if not self._silent: loginfo("[%s] System Transitioned, starting work", self._name) else: logerr( "[%s] StateClient received a new state command, that is not understandable", self._name)
class QualitativeMove(PNPSimplePluginServer): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self.listener = tf.TransformListener() self.target_frame = rospy.get_param("~target_frame", "base_link") with open(rospy.get_param("~config_file"), 'r') as f: self.distance = yaml.load(f)["distances"] self.move_client = SimpleActionClient("move_base", MoveBaseAction) self._ps = PNPSimplePluginServer( name=name, ActionSpec=QualitativeMovePepperAction, execute_cb=self.execute_cb, auto_start=False) rospy.loginfo("Creating tracker client") self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction) self.start_client.wait_for_server() rospy.loginfo("Tracker client connected") self._ps.start() rospy.loginfo("... done") def execute_cb(self, goal): self.start_client.send_goal(TrackPersonGoal(id=goal.id, no_turn=True)) try: msg = rospy.wait_for_message("/naoqi_driver_node/people_detected", PersonDetectedArray, timeout=5.) except rospy.ROSException as e: rospy.logwarn(e) self._ps.set_aborted() else: int_id = int(goal.id.split("_")[1]) for p in msg.person_array: if p.id == int_id: try: t = self.listener.getLatestCommonTime( self.target_frame, msg.header.frame_id) p_pose = PoseStamped(header=msg.header, pose=p.person.position) p_pose.header.stamp = t bl_pose = self.listener.transformPose( self.target_frame, p_pose) except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex: rospy.logwarn(ex) else: target_dist = p.person.distance - self.distance[ goal.to] d = target_dist / p.person.distance theta = math.atan2(bl_pose.pose.position.y, bl_pose.pose.position.x) target_pose = bl_pose target_pose.pose.position.x *= d target_pose.pose.position.y *= d target_pose.pose.orientation.x = 0. target_pose.pose.orientation.y = 0. target_pose.pose.orientation.z = math.sin(theta / 2.) target_pose.pose.orientation.w = math.cos(theta / 2.) print target_pose self.move_client.send_goal_and_wait( MoveBaseGoal(target_pose=target_pose)) finally: break res = QualitativeMovePepperResult() res.result.append( ActionResult(cond="robot_at_home", truth_value=False)) res.result.append( ActionResult(cond="robot_pose_unknown", truth_value=True)) self._ps.set_succeeded(res)
class MyFrame(wx.Frame): def __init__(self,parent,id): the_size=(700, 520) wx.Frame.__init__(self,parent,id,'Elfin Control Panel',pos=(250,100)) self.panel=wx.Panel(self) font=self.panel.GetFont() font.SetPixelSize((12, 24)) self.panel.SetFont(font) self.listener = tf.TransformListener() self.robot=moveit_commander.RobotCommander() self.scene=moveit_commander.PlanningSceneInterface() self.group=moveit_commander.MoveGroupCommander('elfin_arm') self.controller_ns='elfin_arm_controller/' self.elfin_driver_ns='elfin_ros_control/elfin/' self.elfin_basic_api_ns='elfin_basic_api/' self.joint_names=rospy.get_param(self.controller_ns+'joints', []) self.js_display=[0]*6 # joint_states self.jm_button=[0]*6 # joints_minus self.jp_button=[0]*6 # joints_plus self.js_label=[0]*6 # joint_states self.ps_display=[0]*6 # pcs_states self.pm_button=[0]*6 # pcs_minus self.pp_button=[0]*6 # pcs_plus self.ps_label=[0]*6 # pcs_states self.display_init() self.key=[] btn_height=390 btn_lengths=[] self.power_on_btn=wx.Button(self.panel, label=' Servo On ', name='Servo On', pos=(20, btn_height)) btn_lengths.append(self.power_on_btn.GetSize()[0]) btn_total_length=btn_lengths[0] self.power_off_btn=wx.Button(self.panel, label=' Servo Off ', name='Servo Off') btn_lengths.append(self.power_off_btn.GetSize()[0]) btn_total_length+=btn_lengths[1] self.reset_btn=wx.Button(self.panel, label=' Clear Fault ') btn_lengths.append(self.reset_btn.GetSize()[0]) btn_total_length+=btn_lengths[2] self.home_btn=wx.Button(self.panel, label='Home', name='home_btn') btn_lengths.append(self.home_btn.GetSize()[0]) btn_total_length+=btn_lengths[3] self.stop_btn=wx.Button(self.panel, label='Stop', name='Stop') btn_lengths.append(self.stop_btn.GetSize()[0]) btn_total_length+=btn_lengths[4] btn_interstice=(550-btn_total_length)/4 btn_pos_tmp=btn_lengths[0]+btn_interstice+20 self.power_off_btn.SetPosition((btn_pos_tmp, btn_height)) btn_pos_tmp+=btn_lengths[1]+btn_interstice self.reset_btn.SetPosition((btn_pos_tmp, btn_height)) btn_pos_tmp+=btn_lengths[2]+btn_interstice self.home_btn.SetPosition((btn_pos_tmp, btn_height)) btn_pos_tmp+=btn_lengths[3]+btn_interstice self.stop_btn.SetPosition((btn_pos_tmp, btn_height)) self.servo_state_label=wx.StaticText(self.panel, label='Servo state:', pos=(590, btn_height-10)) self.servo_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', pos=(600, btn_height+10)) self.servo_state=bool() self.fault_state_label=wx.StaticText(self.panel, label='Fault state:', pos=(590, btn_height+60)) self.fault_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', pos=(600, btn_height+80)) self.fault_state=bool() self.reply_show_label=wx.StaticText(self.panel, label='Result:', pos=(20, btn_height+60)) self.reply_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', size=(550, 30), pos=(20, btn_height+80)) # the variables about velocity scaling velocity_scaling_init=rospy.get_param(self.elfin_basic_api_ns+'velocity_scaling', default=0.4) default_velocity_scaling=str(round(velocity_scaling_init, 2)) self.velocity_setting_label=wx.StaticText(self.panel, label='Velocity Scaling', pos=(20, btn_height-70)) self.velocity_setting=wx.Slider(self.panel, value=int(velocity_scaling_init*100), minValue=1, maxValue=100, style = wx.SL_HORIZONTAL, size=(500, 30), pos=(45, btn_height-50)) self.velocity_setting_txt_lower=wx.StaticText(self.panel, label='1%', pos=(20, btn_height-45)) self.velocity_setting_txt_upper=wx.StaticText(self.panel, label='100%', pos=(550, btn_height-45)) self.velocity_setting_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER|wx.TE_READONLY), value=default_velocity_scaling, pos=(600, btn_height-55)) self.velocity_setting.Bind(wx.EVT_SLIDER, self.velocity_setting_cb) self.teleop_api_dynamic_reconfig_client=dynamic_reconfigure.client.Client(self.elfin_basic_api_ns, config_callback=self.basic_api_reconfigure_cb) self.dlg=wx.Dialog(self.panel, title='messag') self.dlg.Bind(wx.EVT_CLOSE, self.closewindow) self.dlg_panel=wx.Panel(self.dlg) self.dlg_label=wx.StaticText(self.dlg_panel, label='hello', pos=(15, 15)) self.call_teleop_joint=rospy.ServiceProxy(self.elfin_basic_api_ns+'joint_teleop', SetInt16) self.call_teleop_joint_req=SetInt16Request() self.call_teleop_cart=rospy.ServiceProxy(self.elfin_basic_api_ns+'cart_teleop', SetInt16) self.call_teleop_cart_req=SetInt16Request() self.call_teleop_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop', SetBool) self.call_teleop_stop_req=SetBoolRequest() self.call_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop', SetBool) self.call_stop_req=SetBoolRequest() self.call_stop_req.data=True self.stop_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_stop, rq=self.call_stop_req : self.call_set_bool_common(evt, cl, rq)) self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool) self.call_reset_req=SetBoolRequest() self.call_reset_req.data=True self.reset_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_reset, rq=self.call_reset_req : self.call_set_bool_common(evt, cl, rq)) self.call_power_on=rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool) self.call_power_on_req=SetBoolRequest() self.call_power_on_req.data=True self.power_on_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_power_on, rq=self.call_power_on_req : self.call_set_bool_common(evt, cl, rq)) self.call_power_off=rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool) self.call_power_off_req=SetBoolRequest() self.call_power_off_req.data=True self.power_off_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_power_off, rq=self.call_power_off_req : self.call_set_bool_common(evt, cl, rq)) self.call_move_homing=rospy.ServiceProxy(self.elfin_basic_api_ns+'home_teleop', SetBool) self.call_move_homing_req=SetBoolRequest() self.call_move_homing_req.data=True self.home_btn.Bind(wx.EVT_LEFT_DOWN, lambda evt, cl=self.call_move_homing, rq=self.call_move_homing_req : self.call_set_bool_common(evt, cl, rq)) self.home_btn.Bind(wx.EVT_LEFT_UP, lambda evt, mark=100: self.release_button(evt, mark) ) self.action_client=SimpleActionClient(self.controller_ns+'follow_joint_trajectory', FollowJointTrajectoryAction) self.action_goal=FollowJointTrajectoryGoal() self.action_goal.trajectory.joint_names=self.joint_names self.SetMinSize(the_size) self.SetMaxSize(the_size) def display_init(self): js_pos=[20, 20] js_btn_length=[70, 70, 61, 80] js_distances=[10, 20, 10, 26] dis_h=50 for i in xrange(len(self.js_display)): self.jp_button[i]=wx.Button(self.panel, label='J'+str(i+1)+' +', pos=(js_pos[0], js_pos[1]+(5-i)*dis_h), size=(70,40)) dis_tmp=js_btn_length[0]+js_distances[0] self.jp_button[i].Bind(wx.EVT_LEFT_DOWN, lambda evt, mark=i+1 : self.teleop_joints(evt, mark) ) self.jp_button[i].Bind(wx.EVT_LEFT_UP, lambda evt, mark=i+1 : self.release_button(evt, mark) ) self.jm_button[i]=wx.Button(self.panel, label='J'+str(i+1)+' -', pos=(js_pos[0]+dis_tmp, js_pos[1]+(5-i)*dis_h), size=(70,40)) dis_tmp+=js_btn_length[1]+js_distances[1] self.jm_button[i].Bind(wx.EVT_LEFT_DOWN, lambda evt, mark=-1*(i+1) : self.teleop_joints(evt, mark) ) self.jm_button[i].Bind(wx.EVT_LEFT_UP, lambda evt, mark=-1*(i+1) : self.release_button(evt, mark) ) pos_js_label=(js_pos[0]+dis_tmp, js_pos[1]+(5-i)*dis_h) self.js_label[i]=wx.StaticText(self.panel, label='J'+str(i+1)+'/deg:', pos=pos_js_label) self.js_label[i].SetPosition((pos_js_label[0], pos_js_label[1]+abs(40-self.js_label[i].GetSize()[1])/2)) dis_tmp+=js_btn_length[2]+js_distances[2] pos_js_display=(js_pos[0]+dis_tmp, js_pos[1]+(5-i)*dis_h) self.js_display[i]=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value=' 0000.00 ', pos=pos_js_display) self.js_display[i].SetPosition((pos_js_display[0], pos_js_display[1]+abs(40-self.js_display[i].GetSize()[1])/2)) dis_tmp+=js_btn_length[3]+js_distances[3] ps_pos=[js_pos[0]+dis_tmp, 20] ps_btn_length=[70, 70, 53, 80] ps_distances=[10, 20, 10, 20] pcs_btn_label=['X', 'Y', 'Z', 'Rx', 'Ry', 'Rz'] pcs_label=['X', 'Y', 'Z', 'R', 'P', 'Y'] unit_label=['/mm:', '/mm:', '/mm:', '/deg:', '/deg:', '/deg:'] for i in xrange(len(self.ps_display)): self.pp_button[i]=wx.Button(self.panel, label=pcs_btn_label[i]+' +', pos=(ps_pos[0], ps_pos[1]+(5-i)*dis_h), size=(70,40)) dis_tmp=ps_btn_length[0]+ps_distances[0] self.pp_button[i].Bind(wx.EVT_LEFT_DOWN, lambda evt, mark=i+1 : self.teleop_pcs(evt, mark) ) self.pp_button[i].Bind(wx.EVT_LEFT_UP, lambda evt, mark=i+1 : self.release_button(evt, mark) ) self.pm_button[i]=wx.Button(self.panel, label=pcs_btn_label[i]+' -', pos=(ps_pos[0]+dis_tmp, ps_pos[1]+(5-i)*dis_h), size=(70,40)) dis_tmp+=ps_btn_length[1]+ps_distances[1] self.pm_button[i].Bind(wx.EVT_LEFT_DOWN, lambda evt, mark=-1*(i+1) : self.teleop_pcs(evt, mark) ) self.pm_button[i].Bind(wx.EVT_LEFT_UP, lambda evt, mark=-1*(i+1) : self.release_button(evt, mark) ) pos_ps_label=(ps_pos[0]+dis_tmp, ps_pos[1]+(5-i)*dis_h) self.ps_label[i]=wx.StaticText(self.panel, label=pcs_label[i]+unit_label[i], pos=pos_ps_label) self.ps_label[i].SetPosition((pos_ps_label[0], pos_ps_label[1]+abs(40-self.ps_label[i].GetSize()[1])/2)) dis_tmp+=ps_btn_length[2]+ps_distances[2] pos_ps_display=(ps_pos[0]+dis_tmp, ps_pos[1]+(5-i)*dis_h) self.ps_display[i]=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', pos=pos_ps_display) self.ps_display[i].SetPosition((pos_ps_display[0], pos_ps_display[1]+abs(40-self.ps_display[i].GetSize()[1])/2)) dis_tmp+=ps_btn_length[3]+ps_distances[3] def velocity_setting_cb(self, event): current_velocity_scaling=self.velocity_setting.GetValue()*0.01 self.teleop_api_dynamic_reconfig_client.update_configuration({'velocity_scaling': current_velocity_scaling}) wx.CallAfter(self.update_velocity_scaling_show, current_velocity_scaling) def basic_api_reconfigure_cb(self, config): if self.velocity_setting_show.GetValue()!=config.velocity_scaling: self.velocity_setting.SetValue(int(config.velocity_scaling*100)) wx.CallAfter(self.update_velocity_scaling_show, config.velocity_scaling) def action_stop(self): self.action_client.wait_for_server() self.action_goal.trajectory.header.stamp.secs=0 self.action_goal.trajectory.header.stamp.nsecs=0 self.action_goal.trajectory.points=[] self.action_client.send_goal(self.action_goal) def teleop_joints(self,event,mark): self.call_teleop_joint_req.data=mark resp=self.call_teleop_joint.call(self.call_teleop_joint_req) wx.CallAfter(self.update_reply_show, resp) event.Skip() def teleop_pcs(self,event,mark): self.call_teleop_cart_req.data=mark resp=self.call_teleop_cart.call(self.call_teleop_cart_req) wx.CallAfter(self.update_reply_show, resp) event.Skip() def release_button(self, event, mark): self.call_teleop_stop_req.data=True resp=self.call_teleop_stop.call(self.call_teleop_stop_req) wx.CallAfter(self.update_reply_show, resp) event.Skip() def call_set_bool_common(self, event, client, request): btn=event.GetEventObject() check_list=['Servo On', 'Servo Off'] if btn.GetName() in check_list: self.show_message_dialog(btn.GetName(), client, request) else: try: resp=client.call(request) wx.CallAfter(self.update_reply_show, resp) except rospy.ServiceException, e: resp=SetBoolResponse() resp.success=False resp.message='no such service in simulation' wx.CallAfter(self.update_reply_show, resp) event.Skip()
class Arm: ''' Interfacing with one arm for controlling mode and action execution''' _is_autorelease_on = False def __init__(self, arm_index, tf_listener): self.arm_index = arm_index self.tf_listener = tf_listener self.arm_mode = ArmMode.HOLD self.gripper_state = None self.gripper_joint_name = self._side_prefix() + '_gripper_joint' self.ee_name = self._side_prefix() + '_wrist_roll_link' self.joint_names = [ self._side_prefix() + '_shoulder_pan_joint', self._side_prefix() + '_shoulder_lift_joint', self._side_prefix() + '_upper_arm_roll_joint', self._side_prefix() + '_elbow_flex_joint', self._side_prefix() + '_forearm_roll_joint', self._side_prefix() + '_wrist_flex_joint', self._side_prefix() + '_wrist_roll_joint' ] self.all_joint_names = [] self.all_joint_poses = [] self.last_ee_pose = None self.movement_buffer_size = 40 self.last_unstable_time = rospy.Time.now() self.arm_movement = [] self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) switch_controller = 'pr2_controller_manager/switch_controller' rospy.wait_for_service(switch_controller) self.switch_service = rospy.ServiceProxy(switch_controller, SwitchController) rospy.loginfo('Got response from the switch controller for ' + self.side() + ' arm.') # # Create a trajectory action client traj_controller_name = (self._side_prefix() + '_arm_controller/joint_trajectory_action') self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) self.traj_action_client.wait_for_server() rospy.loginfo('Got response from trajectory action server for ' + self.side() + ' arm.') # Set up Inversse Kinematics self.ik_srv = None self.ik_request = None self.ik_joints = None self.ik_limits = None self._setup_ik() gripper_name = (self._side_prefix() + '_gripper_controller/gripper_action') self.gripper_client = SimpleActionClient(gripper_name, Pr2GripperCommandAction) self.gripper_client.wait_for_server() rospy.loginfo('Got response from gripper server for ' + self.side() + ' arm.') self.check_gripper_state() def _setup_ik(self): '''Sets up services for inverse kinematics''' ik_srv_name = '/compute_ik' rospy.loginfo('IK info service has responded for ' + self.side() + ' arm.') rospy.wait_for_service(ik_srv_name) self.ik_srv = rospy.ServiceProxy(ik_srv_name, GetPositionIK, persistent=True) rospy.loginfo('IK service has responded for ' + self.side() + ' arm.') robot = moveit_commander.RobotCommander() # Set up common parts of an IK request self.ik_request = GetPositionIKRequest() request = self.ik_request.ik_request request.timeout = rospy.Duration(4) group_name = self.side() + '_arm' request.group_name = group_name self.ik_joints = self.joint_names self.ik_limits = [robot.get_joint(x).bounds() for x in self.ik_joints] request.ik_link_name = self.ee_name request.pose_stamped.header.frame_id = 'base_link' request.robot_state.joint_state.name = self.ik_joints request.robot_state.joint_state.position = [0] * len(self.joint_names) def side(self): '''Returns the word right or left depending on arm side''' if (self.arm_index == Side.RIGHT): return 'right' elif (self.arm_index == Side.LEFT): return 'left' def _side_prefix(self): ''' Returns the letter r or l depending on arm side''' side = self.side() return side[0] def get_ee_state(self, ref_frame='base_link'): ''' Returns end effector pose for the arm''' try: time = self.tf_listener.getLatestCommonTime( ref_frame, self.ee_name) (position, orientation) = self.tf_listener.lookupTransform( ref_frame, self.ee_name, time) tf_pose = Pose() tf_pose.position = Point(position[0], position[1], position[2]) tf_pose.orientation = Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) return tf_pose except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logwarn('Something wrong with transform request: ' + str(e)) return None 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.lock.release() def get_joint_state(self, joint_names=None): '''Returns position for the requested or all arm joints''' if joint_names is None: joint_names = self.joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received!\n") return [] 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 positions def _solve_ik(self, ee_pose, seed=None): '''Gets the IK solution for end effector pose''' self.ik_request.ik_request.pose_stamped.pose = ee_pose if seed is None: # If no see is specified for IK search, start search at midpoint seed = [] for i in range(0, len(self.ik_joints)): seed.append( (self.ik_limits[i][0] + self.ik_limits[i][1]) / 2.0) if ee_pose is None: rospy.logerr('End effector pose was None!') if seed is None: rospy.logerr('Seed is still None!') self.ik_request.ik_request.robot_state.joint_state.position = seed try: #rospy.loginfo('Sending IK request.') response = self.ik_srv(self.ik_request) if (response.error_code.val == response.error_code.SUCCESS): # The solution contains all robot joints, we only need the joints of one arm. response_names = response.solution.joint_state.name response_positions = response.solution.joint_state.position return [ response_positions[i] for i, x in enumerate(response_names) if x in self.joint_names ] else: return None except Exception: rospy.logerr('Exception while getting the IK solution.') return None def set_mode(self, mode): '''Releases or holds the arm by turning the controller on/off''' controller_name = self._side_prefix() + '_arm_controller' if mode == ArmMode.RELEASE: start_controllers = [] stop_controllers = [controller_name] rospy.loginfo('Switching ' + str(self.side()) + ' arm to the kinesthetic mode') elif mode == ArmMode.HOLD: start_controllers = [controller_name] stop_controllers = [] rospy.loginfo('Switching ' + str(self.side()) + ' to the Joint-control mode') else: rospy.logwarn('Unknown mode ' + str(mode) + '. Keeping the current mode.') return try: self.switch_service(start_controllers, stop_controllers, 1) self.arm_mode = mode except rospy.ServiceException: rospy.logerr("Service did not process request") def get_mode(self): '''Returns the current arm mode (released/holding)''' return self.arm_mode def _send_gripper_command(self, pos=0.08, eff=30.0, wait=True): '''Sets the position of the gripper''' command = Pr2GripperCommandGoal() command.command.position = pos command.command.max_effort = eff self.gripper_client.send_goal(command) if wait: self.gripper_client.wait_for_result(rospy.Duration(5.0)) def is_gripper_moving(self): ''' Whether or not the gripper is in the process of opening/closing''' return (self.gripper_client.get_state() == GoalStatus.ACTIVE or self.gripper_client.get_state() == GoalStatus.PENDING) def is_gripper_at_goal(self): ''' Whether or not the gripper has reached its goal''' return (self.gripper_client.get_state() == GoalStatus.SUCCEEDED) def get_gripper_state(self): '''Returns current gripper state''' return self.gripper_state def get_gripper_position(self): joint_name = self.gripper_joint_name gripper_pos = self.get_joint_state([joint_name]) if gripper_pos != []: return gripper_pos[0] def check_gripper_state(self, joint_name=None): '''Checks gripper state at the hardware level''' if (joint_name is None): joint_name = self.gripper_joint_name gripper_pos = self.get_joint_state([joint_name]) if gripper_pos != []: if gripper_pos[0] > 0.078: self.gripper_state = GripperState.OPEN else: self.gripper_state = GripperState.CLOSED else: rospy.logwarn('Could not update the gripper state.') def open_gripper(self, pos=0.08, eff=30.0, wait=True): '''Opens gripper''' self._send_gripper_command(pos, eff, wait) self.gripper_state = GripperState.OPEN def close_gripper(self, pos=0.0, eff=30.0, wait=True): '''Closes gripper''' self._send_gripper_command(pos, eff, wait) self.gripper_state = GripperState.CLOSED def set_gripper(self, gripper_state): '''Sets gripper to the desired state''' if (gripper_state == GripperState.CLOSED): self.close_gripper() elif (gripper_state == GripperState.OPEN): self.open_gripper() def move_to_joints(self, joints, time_to_joint): '''Moves the arm to the desired joints''' # Setup the goal traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.joint_names = self.joint_names velocities = [0] * len(joints) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=joints, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) self.traj_action_client.send_goal(traj_goal) def get_time_to_pose(self, target_pose): '''Returns the time to get to the arm pose held in target_pose. Args: target_pose (Pose|None): A Pose holding the pose to move to, or None if the arm should not move. Returns: float|None: How long (in seconds) to allow for moving arm to the pose in target_pose, or None if the arm will not move. ''' # Get readable strings representing the referred arm. arm_name_lower = self.side() arm_name_cap = arm_name_lower.capitalize() # Check whether arm will move at all. if target_pose is None: rospy.loginfo('\t' + arm_name_cap + ' arm will not move.') return None else: time_to_pose = Arm._get_time_bw_poses(self.get_ee_state(), target_pose) rospy.loginfo('\tDuration until next frame for ' + arm_name_lower + 'arm : ' + str(time_to_pose)) return time_to_pose @staticmethod def _get_time_bw_poses(pose0, pose1, velocity=0.2): '''Determines how much time should be allowed for moving between pose0 and pose1 at velocity. Args: pose0 (Pose) pose1 (Pose) velocity (float, optional): Defaults to 0.2. Returns: float: How long (in seconds) to allow for moving between pose0 and pose1 and velocity. ''' dist = Arm.get_distance_bw_poses(pose0, pose1) duration = dist / velocity return (DURATION_MIN_THRESHOLD if duration < DURATION_MIN_THRESHOLD else duration) #TODO def is_executing(self): '''Whether or not there is an ongoing action execution on the arm''' return (self.traj_action_client.get_state() == GoalStatus.ACTIVE or self.traj_action_client.get_state() == GoalStatus.PENDING) #TODO def is_successful(self): '''Whetehr the execution succeeded''' return (self.traj_action_client.get_state() == GoalStatus.SUCCEEDED) def get_ik_for_ee(self, ee_pose, seed): ''' Finds the IK solution for given end effector pose''' joints = self._solve_ik(ee_pose, seed) ## If our seed did not work, try once again with the default seed if joints is None: #rospy.logwarn('Could not find IK solution with preferred seed,' + # 'will try default seed.') joints = self._solve_ik(ee_pose) if joints is None: pass #rospy.logwarn('IK out of bounds, will use the seed directly.') else: rollover = array((array(joints) - array(seed)) / pi, int) joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi return joints @staticmethod def get_distance_bw_poses(pose0, pose1): '''Returns the dissimilarity between two end-effector poses''' w_pos = 1.0 w_rot = 0.2 pos0 = array((pose0.position.x, pose0.position.y, pose0.position.z)) pos1 = array((pose1.position.x, pose1.position.y, pose1.position.z)) rot0 = array((pose0.orientation.x, pose0.orientation.y, pose0.orientation.z, pose0.orientation.w)) rot1 = array((pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w)) pos_dist = w_pos * norm(pos0 - pos1) rot_dist = w_rot * (1 - dot(rot0, rot1)) if (pos_dist > rot_dist): dist = pos_dist else: dist = rot_dist return dist def reset_movement_history(self): ''' Clears the saved history of arm movements''' self.last_unstable_time = rospy.Time.now() self.arm_movement = [] def get_movement(self): '''Returns cumulative movement in recent history''' return sum(self.arm_movement) def _record_arm_movement(self, reading): '''Records the sensed arm movement''' self.arm_movement = [reading] + self.arm_movement if (len(self.arm_movement) > self.movement_buffer_size): self.arm_movement = self.arm_movement[0:self.movement_buffer_size] def _is_arm_moved_while_holding(self): '''Checks if user is trying to move the arm while it is stiff''' threshold = 0.02 if (self.get_mode() == ArmMode.HOLD and (len(self.arm_movement) == self.movement_buffer_size) and (self.get_movement() > threshold)): return True return False def _is_arm_stable_while_released(self): '''Checks if the arm has been stable while being released''' movement_threshold = 0.02 time_threshold = rospy.Duration(5.0) is_arm_stable = (self.get_movement() < movement_threshold) if (not is_arm_stable or self.get_mode() == ArmMode.HOLD): self.last_unstable_time = rospy.Time.now() return False else: if (rospy.Time.now() - self.last_unstable_time) > time_threshold: return True else: return False def update(self, is_executing): ''' Periodical update for one arm''' ee_pose = self.get_ee_state() if (ee_pose != None and self.last_ee_pose != None): self._record_arm_movement( Arm.get_distance_bw_poses(ee_pose, self.last_ee_pose)) self.last_ee_pose = ee_pose if (not is_executing and Arm._is_autorelease_on): if (self._is_arm_moved_while_holding()): rospy.loginfo('Automatically releasing arm.') self.set_mode(ArmMode.RELEASE) if (self._is_arm_stable_while_released()): rospy.loginfo('Automatically holding arm.') self.set_mode(ArmMode.HOLD)
class IK: 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' ] 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' ] def __init__(self, side_prefix): self.side_prefix = side_prefix # Set up Inversse Kinematics services ik_info_srv_name = ('pr2_' + self._side() + '_arm_kinematics_simple/get_ik_solver_info') ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik' rospy.loginfo('Waiting for IK info service to respond.') rospy.wait_for_service(ik_info_srv_name) ik_info_srv = rospy.ServiceProxy(ik_info_srv_name, GetKinematicSolverInfo) solver_info = ik_info_srv() self.ik_joints = solver_info.kinematic_solver_info.joint_names self.ik_limits = solver_info.kinematic_solver_info.limits rospy.loginfo('Waiting for IK service to respond.') rospy.wait_for_service(ik_srv_name) self.ik_srv = rospy.ServiceProxy(ik_srv_name, GetPositionIK, persistent=True) # Set up common parts of an IK request self.ik_request = GetPositionIKRequest() self.ik_request.timeout = rospy.Duration(4.0) self.ik_request.ik_request.ik_link_name = solver_info.kinematic_solver_info.link_names[ 0] self.ik_request.ik_request.pose_stamped.header.frame_id = 'base_link' self.ik_request.ik_request.ik_seed_state.joint_state.name = self.ik_joints self.ik_request.ik_request.ik_seed_state.joint_state.position = [ 0 ] * len(self.ik_joints) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.traj_action_client = SimpleActionClient(motion_request_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory motion request server on arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('IK ready for arm: ' + side_prefix) def _side(self): if (self.side_prefix == 'r'): return 'right' else: return 'left' def move_to_joints(self, positions, time_to_joint): velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (self.side_prefix == 'r'): traj_goal.trajectory.joint_names = IK.r_joint_names else: traj_goal.trajectory.joint_names = IK.l_joint_names self.traj_action_client.send_goal(traj_goal) def get_ik_for_ee(self, ee_pose, seed=None): ''' Finds the IK solution for given end effector pose''' self.ik_request.ik_request.pose_stamped.pose = ee_pose if seed == None: seed = [] for i in range(0, len(self.ik_joints)): seed.append((self.ik_limits[i].min_position + self.ik_limits[i].max_position) / 2.0) self.ik_request.ik_request.ik_seed_state.joint_state.position = seed try: rospy.loginfo('Requesting IK.') response = self.ik_srv(self.ik_request) if (response.error_code.val == response.error_code.SUCCESS): joints = response.solution.joint_state.position else: rospy.logwarn('Could not find IK solution.') return None except rospy.ServiceException: rospy.logerr('Exception while getting the IK solution.') return None rollover = array((array(joints) - array(seed)) / pi, int) joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi return joints
class Navigation: def __init__(self): # initialise state variables self._reset() # services self.robot_pose = Pose() s = rospy.Service('navigate_to', NavigateTo, self.setNavigate) # pub/sub self.nav_done_pub = rospy.Publisher('nav_done', Bool, queue_size=10) rospy.Subscriber('robot_pose', PoseWithCovarianceStamped, self._pose_callback) # NavigateTo self.move_base_msg = MoveBaseGoal() self.move_base_msg.target_pose.header.frame_id = 'map' self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) self.move_base_client.wait_for_server() rospy.loginfo("Ready to navigate") def _pose_callback(self, msg): self.robot_pose = msg.pose.pose def _reset(self): self.state = State.IDLE self.goal_pose = None self.move_base_request_sent = False ### service functions def setExplore(self): self.state = State.EXPLORING def setNavigate(self, msg): self._reset() self.state = State.NAVIGATING self.goal_pose = msg.pose return NavigateToResponse(True) def setFollow(self): self.state = State.FOLLOWING def setGuide(self): self.state = State.GUIDING ### state based logic def explore(self): return def navigate(self): if not self.move_base_request_sent: # create a request to move base self.move_base_msg.target_pose.header.stamp = rospy.Time.now() self.move_base_msg.target_pose.pose = self.goal_pose self.move_base_client.send_goal(self.move_base_msg) rospy.loginfo('Request sent, waiting for planner to move') self.move_base_request_sent = True # Note: I tried using callbacks in send_goal, and using wait_for_response, neither worked and caused the module to wait forever. # The only active topic in the move_base package seems to be status which can be continously monitored until it reports that the goal has been reached. # This is not a good solution, but since the correct mechanisms are broken I don't know what else to do. # Annoyingly something is printing "GOAL Reached!" so there should be a notification of success... I just don't know where it is... # Workaround for now is to reimplement the distance check here which is not good. # self.move_base_client.wait_for_result() # rospy.loginfo(self.move_base_client.get_result) else: # Wait for path to be completed # Note hacky solution to above comment xdiff = (self.goal_pose.position.x - self.robot_pose.position.x)**2 ydiff = (self.goal_pose.position.y - self.robot_pose.position.y)**2 dist = math.sqrt(xdiff + ydiff) if dist < 0.2: # 0.2 value comes from YAML file for the local planner and is the xy_goal_tollerance # notify system of success/failure # failure will only be available if the interface with move_base is fixed self.nav_done_pub.publish(True) rospy.loginfo("Nav done!") # reset nav state machine self._reset() def follow(self): return def guide(self): return def step(self): # check collisions TODO # execute behaviour if self.state == State.EXPLORING: self.explore() elif self.state == State.NAVIGATING: self.navigate() elif self.state == State.FOLLOWING: self.follow() elif self.state == State.GUIDING: self.guide()
class PickAruco(object): def __init__(self): rospy.loginfo("Initalizing...") self.bridge = CvBridge() self.tfBuffer = tf2_ros.Buffer() self.tf_l = tf2_ros.TransformListener(self.tfBuffer) rospy.loginfo("Waiting for /pickup_pose AS...") self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction) #self.force = rospy.Subscriber("/wrist_tf", WrenchStamped, self.force_value) time.sleep(1.0) if not self.pick_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /pickup_pose AS") exit() rospy.loginfo("Waiting for /place_pose AS...") self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction) self.place_as.wait_for_server() rospy.loginfo("Setting publishers to torso and head controller...") self.torso_cmd = rospy.Publisher( '/torso_controller/command', JointTrajectory, queue_size=1) self.head_cmd = rospy.Publisher( '/head_controller/command', JointTrajectory, queue_size=1) self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose', PoseStamped, queue_size=1, latch=True) rospy.loginfo("Waiting for '/play_motion' AS...") self.place_finish = rospy.ServiceProxy( '/god_watcher/place_finish', send_flags) #self.place_finish = SimpleActionClient('/god_watcher/place_finish', send_flags) self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction) self.speak = rospy.ServiceProxy('/tiago/speech/tts', srvTTS) self.speak.wait_for_service() rospy.loginfo("Speak node Connected.") if not self.play_m_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /play_motion AS") exit() rospy.loginfo("Connected!") rospy.sleep(1.0) rospy.loginfo("Done initializing PickAruco.") def strip_leading_slash(self, s): return s[1:] if s.startswith("/") else s def force_value(self, data): force_x = np.absolute(data.wrench.force.x) force_y = np.absolute(data.wrench.force.y) force_z = np.absolute(data.wrench.force.z) force_abs = force_x + force_y + force_z rospy.loginfo("I have force_abs") if force_abs >= 10.0: rospy.loginfo("I got the bag.") def tiago_speak(self, string_text): rospy.loginfo(string_text) msg_speech = srvTTSRequest() msg_speech.text = string_text self.speak.call(msg_speech) rospy.sleep(0.1) rospy.loginfo("Done talking.") def pick_aruco(self, string_operation): self.prepare_robot() rospy.sleep(2.0) rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection") #rospy.sleep(5.0) aruco_pose = rospy.wait_for_message('/object_detection/Pick_Pose', PoseStamped) aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id) rospy.loginfo("Got: " + str(aruco_pose)) rospy.loginfo("spherical_grasp_gui: Transforming from frame: " + aruco_pose.header.frame_id + " to 'base_footprint'") ps = PoseStamped() ps.pose.position = aruco_pose.pose.position ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id) ps.header.frame_id = aruco_pose.header.frame_id transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: transform = self.tfBuffer.lookup_transform("base_footprint", ps.header.frame_id, rospy.Time(0)) aruco_ps = do_transform_pose(ps, transform) transform_ok = True except tf2_ros.ExtrapolationException as e: rospy.logwarn( "Exception on transforming point... trying again \n(" + str(e) + ")") rospy.sleep(0.01) ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id) pick_g = PickUpPoseGoal() if string_operation == "pick": rospy.loginfo("Setting cube pose based on bottle detection") pick_g.object_pose.pose.position.x = aruco_ps.pose.position.x + 0.04 pick_g.object_pose.pose.position.y = aruco_ps.pose.position.y - 0.04 pick_g.object_pose.pose.position.z = aruco_ps.pose.position.z + 0.14 # pick_g.object_pose.pose.orientation = aruco_ps.pose.position # pick_g.object_pose.pose.position = aruco_ps.pose.position # pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) rospy.loginfo("aruco pose in base_footprint:" + str(pick_g)) pick_g.object_pose.header.frame_id = 'base_footprint' pick_g.object_pose.pose.orientation.w = 1.0 self.detected_pose_pub.publish(pick_g.object_pose) rospy.loginfo("Gonna pick:" + str(pick_g)) self.pick_as.send_goal_and_wait(pick_g) rospy.loginfo("Done!") result = self.pick_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr("Failed to pick, not trying further") return rospy.loginfo("######before") # Move torso to its maximum height self.lift_torso() # Raise arm #rospy.loginfo("Moving arm to a safe pose") pmg = PlayMotionGoal() pmg.motion_name = 'pick_final_pose' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) data = rospy.wait_for_message('/wrist_ft', WrenchStamped) force_x = np.absolute(data.wrench.force.x) force_y = np.absolute(data.wrench.force.y) force_z = np.absolute(data.wrench.force.z) force_abs = force_x + force_y + force_z rospy.loginfo("I have force_abs")
#!/usr/bin/env python import roslib roslib.load_manifest('rospy') roslib.load_manifest('actionlib') roslib.load_manifest('actionlib_msgs') roslib.load_manifest('control_msgs') import rospy from control_msgs.msg import GripperCommandAction from control_msgs.msg import GripperCommandGoal from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus if __name__ == "__main__": rospy.init_node('gripper_test_node', anonymous=True) name_space = '/r_gripper_controller/gripper_action' gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_client.wait_for_server() gripper_goal = GripperCommandGoal() gripper_goal.command.position = 0.00 #Opens the gripper, use 0.0 to close it. gripper_goal.command.max_effort = 30.0 gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(10.0)) if (gripper_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Gripper action unsuccessful.')