def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016) self._arm_group = rospy.get_param('~arm', 'arm_move_group') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) # Create planning scene where we will add the objects etc. self._scene = PlanningSceneInterface() # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene (remove the old objects: self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: # TODO get the position of the detected object self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name) rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) 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 # Pick 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')
class ReadyArmActionServer: def __init__(self): self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.ready_arm_server = SimpleActionServer(ACTION_NAME, ReadyArmAction, execute_cb=self.ready_arm, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME) self.ready_arm_server.start() def ready_arm(self, goal): if self.ready_arm_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.move_arm_client.cancel_goal() self.ready_arm_server.set_preempted() if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, READY_POSITION, collision_operations=goal.collision_operations): self.ready_arm_server.set_succeeded() else: rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME) self.ready_arm_server.set_aborted()
def 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
def __init__(self): self.action_index = 0 self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction) self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction) self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
class ExplorationController: def __init__(self): self._plan_service = rospy.ServiceProxy('get_exploration_path', GetRobotTrajectory) self._move_base = SimpleActionClient('move_base', MoveBaseAction) def run(self): r = rospy.Rate(1 / 7.0) while not rospy.is_shutdown(): self.run_once() r.sleep() def run_once(self): path = self._plan_service().trajectory poses = path.poses if not path.poses: rospy.loginfo('No frontiers left.') return rospy.loginfo('Moving to frontier...') self.move_to_pose(poses[-1]) def move_to_pose(self, pose_stamped, timeout=20.0): goal = MoveBaseGoal() goal.target_pose = pose_stamped self._move_base.send_goal(goal) self._move_base.wait_for_result(rospy.Duration(timeout)) return self._move_base.get_state() == GoalStatus.SUCCEEDED
def __init__(self): rospy.loginfo("Initializing ChipBehaviour") moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander("right_arm_torso") self.pose_pub = rospy.Publisher('/arm_pointing_pose', geometry_msgs.msg.PoseStamped, queue_size=1) self.tts_ac = SimpleActionClient('/tts', TtsAction) self.tts_ac.wait_for_server() self.point_head_ac = SimpleActionClient('/head_controller/point_head_action', PointHeadAction) self.point_head_ac.wait_for_server(rospy.Duration(10.0)) self.faces_sub = rospy.Subscriber('/pal_face/faces', FaceDetections, self.faces_cb, queue_size=1)
def __init__(self, name): # stuff for grasp planning self.tf_listener = tf.TransformListener() self.tf_broadcaster = tf.TransformBroadcaster() self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_objects = RecognizedObjectArray() #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback) self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback) self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray) rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) #pickup_ac.wait_for_server() # needed? rospy.loginfo("Connecting to grasp generator AS") self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction) #grasps_ac.wait_for_server() # needed? #planning scene for motion planning self.scene = PlanningSceneInterface() # blocking action server self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False) self.feedback = GraspObjectFeedback() self.result = GraspObjectResult() self.current_goal = None self.grasp_obj_as.start()
def __init__(self): self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self.all_joint_names = [] self.all_joint_poses = [] self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server()
class StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.move_client = SimpleActionClient('/move_to', MoveAction) self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect) self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result)) return try: self.change_wrench('autonomous') rospy.loginfo('Now station holding') self.broadcaster.clear_alarm() except rospy.ServiceException as e: rospy.logwarn('Station hold changing wrench failed: {}'.format(e)) def raised(self, alarm): rospy.loginfo("Attempting to station hold") station_hold_goal = MoveGoal() station_hold_goal.move_type = 'hold' self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
def __init__(self): rospy.loginfo('__init__ start') # create a node rospy.init_node(NODE) # Action server to receive goals self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, self.handle_path, auto_start=False) self.path_server.start() # publishers & clients self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path) # get parameters from launch file self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True) # action server based on use_obstacle_avoidance if self.use_obstacle_avoidance == False: self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction) else: self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction) self.goal_client.wait_for_server() # other fields self.goal_index = 0 self.executePathGoal = None self.executePathResult = ExecutePathResult()
def 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 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 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)
def __init__(self, file): self.fileOut = file rospy.init_node("object_swatter", anonymous=True) self.all_objects = [] self.all_object_names = [] self.current_obj_idx = -1 self.swat_time = rospy.Time.now() self.model_data = None self.blockToSwat = "" self.min_front_dist = 10.0 self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist) self.allPoints = [] self.inProgress = False # rospy.Subscriber('overhead_objects', PoseArray, self.update_object_positions) rospy.Subscriber("gazebo/model_states", ModelStates, self.read_model) # self.sh_pan_speed_srv = rospy.ServiceProxy('shoulder_pan_controller/set_speed', SetSpeed) self.base_client = SimpleActionClient("erratic_base_action", ErraticBaseAction) self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction) self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction) rospy.Subscriber("tilt_laser/scan", LaserScan, self.filter_scan) # rospy.wait_for_service('shoulder_pan_controller/set_speed') self.base_client.wait_for_server() self.arm_client.wait_for_server() self.gripper_client.wait_for_server() rospy.loginfo("Connected to all action servers")
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
def __init__(self): # Initialize new node rospy.init_node(NAME)#, anonymous=False) #initialize the clients to interface with self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction) self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction) self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction) self.move_client.wait_for_server() self.arm_client.wait_for_server() self.gripper_client.wait_for_server() # Initialize tf listener (remove?) self.tf = tf.TransformListener() # Initialize erratic base action server self.result = SmartArmGraspResult() self.feedback = SmartArmGraspFeedback() self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback) #define the offsets # These offsets were determines expirmentally using the simulator # They were tested using points stamped with /map self.xOffset = 0.025 self.yOffset = 0.0 self.zOffset = 0.12 #.05 # this does work! rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME) rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def __init__(self): self.all_objects = [] self.current_obj_idx = -1 self.swat_time = rospy.Time.now() self.min_dist = 10 self.front_dist = 10 self.avg_front_dist = 10 self.min_front_dist = 10 rospy.Subscriber('overhead_objects', PoseArray, self.update_object_positions) rospy.Subscriber('tilt_laser/scan', LaserScan, self.filter_scan) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) self.sh_pan_speed_srv = rospy.ServiceProxy('shoulder_pan_controller/set_speed', SetSpeed) self.classify_obj_srv = rospy.ServiceProxy('classify_object', ClassifyObject) self.base_client = SimpleActionClient('erratic_base_action', ErraticBaseAction) self.arm_client = SimpleActionClient('smart_arm_action', SmartArmAction) self.gripper_client = SimpleActionClient('smart_arm_gripper_action', SmartArmGripperAction) rospy.wait_for_service('shoulder_pan_controller/set_speed') rospy.wait_for_service('classify_object') self.base_client.wait_for_server() self.arm_client.wait_for_server() self.gripper_client.wait_for_server() rospy.loginfo('Connected to all action servers')
def __init__(self, joint_mover): rospy.loginfo("Waiting for the move_arm actions") self.move_right_arm_client = SimpleActionClient("move_right_arm", MoveArmAction) self.move_right_arm_client.wait_for_server() self.move_left_arm_client = SimpleActionClient("move_left_arm", MoveArmAction) self.move_left_arm_client.wait_for_server() rospy.loginfo("waiting for action make_static_collision_map action server") self.make_static_collision_map_client = actionlib.SimpleActionClient( 'make_static_collision_map', MakeStaticCollisionMapAction) self.make_static_collision_map_client.wait_for_server() rospy.loginfo("waiting for service set_planning_scene_diff") self.planning_scene_client = rospy.ServiceProxy( "/environment_server/set_planning_scene_diff", SetPlanningSceneDiff) self.planning_scene_client.wait_for_service() self.tf_listener = tf.TransformListener() self.right_ik = pr2_control_utilities.IKUtilities("right", tf_listener=self.tf_listener) self.left_ik = pr2_control_utilities.IKUtilities("left", tf_listener=self.tf_listener) self.joint_mover = joint_mover rospy.loginfo("%s is ready", self.__class__.__name__)
def __init__(self): self._goal = "" self._aISeq = 0 rospy.logdebug("Initializing KeyframeExecutor") rospy.logdebug("Waiting for set_effector_position") rospy.wait_for_service('set_effector_position') self._setEffectorPositionProxy = rospy.ServiceProxy( "set_effector_position", SetEffectorPosition) rospy.logdebug("Waiting for set_effector_positions") rospy.wait_for_service('set_effector_positions') self._setEffectorPositionsProxy = rospy.ServiceProxy( "set_effector_positions", SetEffectorPositions) self._postureClient = SimpleActionClient('body_pose_naoqi', BodyPoseWithSpeedAction) rospy.logdebug("Waiting for body_pose_naoqi") self._postureClient.wait_for_server() self._client = SimpleActionClient("joint_trajectory", JointTrajectoryAction) rospy.logdebug("Waiting for joint_trajectory") self._client.wait_for_server() self._functionLookUp = { "angleInterpolation" : self._angleInterpolation, "wait" : self._wait, "setEffectorPosition" : self._setEffectorPosition, "setEffectorPositions" : self._setEffectorPositions, "posture" : self._posture }
def __init__(self): super(Pr2LookAtFace, self).__init__() self._client = SimpleActionClient('face_detector_action',FaceDetectorAction) self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction) self._timer = None self._executing = False self._pending_face_goal = False self._pending_head_goal = False
def __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)
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
def __init__(self, ik, joint_mover): self.move_right_arm_client = SimpleActionClient("move_right_arm", MoveArmAction) self.move_right_arm_client.wait_for_server() self.move_left_arm_client = SimpleActionClient("move_left_arm", MoveArmAction) self.move_left_arm_client.wait_for_server() self.ik = ik self.joint_mover = joint_mover
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 __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...')
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 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 __init__(self, robot_name, side, tf_listener): self.robot_name = robot_name self.side = side if (self.side is "left") or (self.side is "right"): pass else: raise Exception("Side should be either: left or right") self.tf_listener = tf_listener self._occupied_by = None self._operational = True # In simulation, there will be no hardware cb # Get stuff from the parameter server self.offset = self.load_param('skills/arm/offset/' + self.side) self.marker_to_grippoint_offset = self.load_param('skills/arm/offset/marker_to_grippoint') self.joint_names = self.load_param('skills/arm/joint_names') self.joint_names = [name + "_" + self.side for name in self.joint_names] self.torso_joint_names = rospy.get_param('/'+self.robot_name+'/skills/torso/joint_names') self.default_configurations = self.load_param('skills/arm/default_configurations') self.default_trajectories = self.load_param('skills/arm/default_trajectories') # listen to the hardware status to determine if the arm is available rospy.Subscriber("/amigo/hardware_status", DiagnosticArray, self.cb_hardware_status) # Init gripper actionlib self._ac_gripper = SimpleActionClient( "/" + robot_name + "/" + self.side + "_arm/gripper/action", GripperCommandAction) # Init graps precompute actionlib self._ac_grasp_precompute = SimpleActionClient( "/" + robot_name + "/" + self.side + "_arm/grasp_precompute", GraspPrecomputeAction) # Init joint trajectory action server self._ac_joint_traj = SimpleActionClient( "/" + robot_name + "/body/joint_trajectory_action", FollowJointTrajectoryAction) # ToDo: don't hardcode? // Comment from @reinzor, this always fails on # startup of action server, why do we need this, who uses this info? server_timeout = 0.25 self._ac_gripper_present = self._ac_gripper.wait_for_server(timeout=rospy.Duration(server_timeout)) if not self._ac_gripper_present: rospy.loginfo("Cannot find gripper {0} server".format(self.side)) self._ac_grasp_precompute_present = self._ac_grasp_precompute.wait_for_server(timeout=rospy.Duration(server_timeout)) if not self._ac_grasp_precompute_present: rospy.loginfo("Cannot find grasp precompute {0} server".format(self.side)) self._ac_joint_traj_present = self._ac_joint_traj.wait_for_server(timeout=rospy.Duration(server_timeout)) if not self._ac_joint_traj_present: rospy.loginfo("Cannot find joint trajectory action server {0}".format(self.side)) # Init marker publisher self._marker_publisher = rospy.Publisher( "/" + robot_name + "/" + self.side + "_arm/grasp_target", visualization_msgs.msg.Marker, queue_size=10)
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()): move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction) move_arm_client.wait_for_server() rospy.loginfo('connected to move_left_arm action server') goal = MoveArmGoal() goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.planner_id = '' goal.motion_plan_request.group_name = 'left_arm' goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)] goal.motion_plan_request.allowed_contacts = allowed_contacts goal.motion_plan_request.link_padding = link_padding goal.motion_plan_request.ordered_collision_operations = collision_operations move_arm_client.send_goal(goal) finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0)) if not finished_within_time: move_arm_client.cancel_goal() rospy.loginfo('timed out trying to achieve a joint goal') else: state = move_arm_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo('action finished: %s' % str(state)) return True else: rospy.loginfo('action failed: %s' % str(state)) return False
def __init__(self): self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482] self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482] self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675] self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919] self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919] self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644] self.v = [0] * len(self.r1) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self._action_name = 'kill' self._tf = tf.TransformListener() #initialize base controller topic_name = '/base_controller/command' self._base_publisher = rospy.Publisher(topic_name, Twist) #Initialize the sound controller self._sound_client = SoundClient() # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.pose = None self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback) #initialize this client self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name)
def __init__(self, name='', topics='', time=0.0): self.client = SimpleActionClient(OnlineBagger.BAG_TOPIC, BagOnlineAction) self.goal = BagOnlineGoal(bag_name=name, topics=topics, bag_time=time) self.result = None self.total_it = 0
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()
#! /usr/bin/env python import rospy from actionlib import SimpleActionClient from rmsc_messages.msg import AggressiveGainBuffAction, AggressiveGainBuffActionGoal def help(): print ''' 1 - SendGoal 2 - CancelGoal 3 - Exit ''' if __name__ == "__main__": rospy.init_node("aggressive_gain_buff_client") ac_ = SimpleActionClient("aggressive_gain_buff_action", AggressiveGainBuffAction) ac_.wait_for_server() print 'Aggressive Gain Buff Sever Connected!' help() while not rospy.is_shutdown(): cmd = raw_input('cmd:') if cmd == '1': route_index = input('Input Route Index:') g = AggressiveGainBuffActionGoal() g.goal.route_index = route_index ac_.send_goal(g.goal) elif cmd == '2': ac_.cancel_all_goals() elif cmd == '3': break else: print 'Invalid Command!'
import tf from actionlib import SimpleActionClient from rmsc_messages.msg import LookAndMoveAction, LookAndMoveActionGoal def help(): print ''' 1 - SendGoal 2 - CancelGoal 3 - Exit ''' if __name__ == "__main__": rospy.init_node("look_n_move_client") ac_ = SimpleActionClient("look_n_move_node_action", LookAndMoveAction) ac_.wait_for_server() print 'sever connected!' help() while not rospy.is_shutdown(): cmd = raw_input('cmd:') if cmd == '1': x = input('offset_x:') y = input('offset_y:') angle = input('offset_angle:') g = LookAndMoveActionGoal() q = tf.transformations.quaternion_from_euler(0., 0., angle) g.goal.relative_pose.pose.position.x = x g.goal.relative_pose.pose.position.y = y g.goal.relative_pose.pose.orientation.z = q[2] g.goal.relative_pose.pose.orientation.w = q[3]
def __init__(self, tf_listener): ''' Args: tf_listener (TransformListener) ''' self._social_gaze = rospy.get_param("/social_gaze") self._play_sound = rospy.get_param("/play_sound") self._tf_listener = tf_listener # arm services self.move_arm_to_joints_plan_srv = rospy.ServiceProxy( 'move_arm_to_joints_plan', MoveArm) rospy.wait_for_service('move_arm_to_joints_plan') self.move_arm_to_joints_srv = rospy.ServiceProxy( 'move_arm_to_joints', MoveArmTraj) rospy.wait_for_service('move_arm_to_joints') self.move_arm_to_pose_srv = rospy.ServiceProxy('move_arm_to_pose', MoveArm) rospy.wait_for_service('move_arm_to_pose') self.start_move_arm_to_pose_srv = rospy.ServiceProxy( 'start_move_arm_to_pose', MoveArm) rospy.wait_for_service('start_move_arm_to_pose') self.is_reachable_srv = rospy.ServiceProxy('is_reachable', MoveArm) rospy.wait_for_service('is_reachable') self.is_arm_moving_srv = rospy.ServiceProxy('is_arm_moving', GetArmMovement) rospy.wait_for_service('is_arm_moving') self.relax_arm_srv = rospy.ServiceProxy('relax_arm', Empty) rospy.wait_for_service('relax_arm') self.reset_arm_movement_history_srv = rospy.ServiceProxy( 'reset_arm_movement_history', Empty) rospy.wait_for_service('reset_arm_movement_history') self.get_gripper_state_srv = rospy.ServiceProxy( 'get_gripper_state', GetGripperState) rospy.wait_for_service('get_gripper_state') self.get_joint_states_srv = rospy.ServiceProxy('get_joint_states', GetJointStates) rospy.wait_for_service('get_joint_states') self.get_ee_pose_srv = rospy.ServiceProxy('get_ee_pose', GetEEPose) rospy.wait_for_service('get_ee_pose') self.set_gripper_state_srv = rospy.ServiceProxy( 'set_gripper_state', SetGripperState) rospy.wait_for_service('set_gripper_state') rospy.loginfo("Got all arm services") # head services self.gaze_client = SimpleActionClient('gaze_action', GazeAction) self.gaze_client.wait_for_server(rospy.Duration(5)) self.current_gaze_goal_srv = rospy.ServiceProxy( 'get_current_gaze_goal', GetGazeGoal) rospy.wait_for_service('get_current_gaze_goal') rospy.loginfo("Got all head services") # world services self._get_nearest_object_srv = rospy.ServiceProxy( 'get_nearest_object', GetNearestObject) rospy.wait_for_service('get_nearest_object') rospy.loginfo("Got all world services") # sound stuff self._sound_client = SoundClient()
laser_client.wait_for_result() result = laser_client.get_result() if result.success == False: print "Action failed" else: print "Result: " + str(result.tilt_position) if __name__ == '__main__': try: x = Swatter() print "Setup Swatter" rospy.init_node(NAME, anonymous=True) head_client = SimpleActionClient("wubble_head_action", WubbleHeadAction) arm_client = SimpleActionClient("smart_arm_action", SmartArmAction) #arm_cient_angled = SimpleActionClient("", ) gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction) laser_client = SimpleActionClient('hokuyo_laser_tilt_action', HokuyoLaserTiltAction) base_client = SimpleActionClient("erratic_base_action", ErraticBaseAction) base_client.wait_for_server() head_client.wait_for_server() arm_client.wait_for_server() gripper_client.wait_for_server() laser_client.wait_for_server() print "Starting block swatting."
def __init__(self, *a, **k): super(PathPlanningMixin, self).__init__(*a, **k) self._path_planner_client = SimpleActionClient('/path_planner', PoseStamped)
def main(argv) -> None: """ Main entry point This design of putting all pub/sub in main function is intentional since we want to avoid pub/sub being instantiated more than once. :param argv: """ rospy.init_node('waypoint_node') # Register publishers first pub_reached = rospy.Publisher("~reached", String, queue_size=1) # FIXME decide queue_size # Register subscribers ds = __DroneStates() # For middleware waypoint_topic_name = "~waypoint" _ = rospy.Subscriber(waypoint_topic_name, PoseStamped, ds.store_waypoint) # Register actionlib clients takeoff_topic = rospy.resolve_name("action/takeoff") takeoff_client = SimpleActionClient(takeoff_topic, TakeoffAction) landing_topic = rospy.resolve_name("action/landing") landing_client = SimpleActionClient(landing_topic, LandingAction) pose_topic = rospy.resolve_name("action/pose") pose_client = SimpleActionClient(pose_topic, PoseAction) def action_pose_done_cb(goal_state, goal_result): rospy.logdebug("Reached\n %s" % str(ds.curr_waypoint.pose.position)) ds.reset_curr_waypoint() def shutdown() -> None: # TODO Better place for this code """Stop the drone when this ROS node shuts down""" # TODO Safe landing pass rospy.on_shutdown(shutdown) # TODO Wait for hector quadrotor controllers to spawn rospy.sleep(1) rate = rospy.Rate(100) # 100Hz TODO Pass sleep rate as a parameter? is_driving = False while not rospy.is_shutdown(): rate.sleep() # Simple controller code for drones # TODO Need better controller if not is_driving: # IDLE if ds._waypoints.empty(): # FIXME accessing protected member pass # Keep idling else: ds.set_curr_waypoint() pose_client.wait_for_server() pose_goal = PoseGoal(target_pose=ds.target_pose()) rospy.logdebug("Sending pose goal\n %s" % str(pose_goal)) pose_client.send_goal(PoseGoal(target_pose=ds.target_pose()), done_cb=action_pose_done_cb) is_driving = True else: # DRIVING if ds.reached == ReachedEnum.NO: pass # Keep driving else: if ds.reached == ReachedEnum.YES_AND_REPORT: pub_reached.publish(ds.report_reached()) is_driving = False
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) table_pose = copy.deepcopy(object_pose) #table_pose1 = copy.deepcopy(object_pose) #table_pose2 = copy.deepcopy(object_pose) #table_pose3 = copy.deepcopy(object_pose) #define a virtual table below the object table_height = 0.24 table_width = 0.20 table_depth = 0.06 table_pose.pose.position.z += -0.30 - table_height / 2 table_pose.pose.position.y += 0.04 #table_height = 0.38 #table_width = 0.30 #table_depth = 0.10 #table_pose.pose.position.z += -0.03 -table_height / 2 - 0.10 #table_pose.pose.position.y += 0.05 #table_height -= 0.02 #remove few milimeters to prevent contact between the object and the table #table_height1 = 0.80 #table_width1 = 0.40 #table_depth1 = 0.35 #table_pose1.pose.position.z += -(self.object_height) / 2 + table_height1 / 2 #table_pose1.pose.position.y += 0.5 #table_height1 -= 0.008 # remove few milimeters to prevent contact between the object and the table #table_height2 = 0.80 #table_width2 = 0.40 #table_depth2 = 0.35 #table_pose2.pose.position.z += -(self.object_height) / 2 + table_height2 / 2 #table_pose2.pose.position.y -= 0.5 #table_height2 -= 0.008 # remove few milimeters to prevent contact between the object and the table #table_height3 = 0.40 #table_width3 = 1.0 #table_depth3 = 0.35 #table_pose3.pose.position.z += -(self.object_height) / 2 + table_height3 / 2 + 0.45 #table_height3 -= 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)) #self.scene.add_box("table1", table_pose1, (table_depth1, table_width1, table_height1)) #add by chen #self.scene.add_box("table2", table_pose2, (table_depth2, table_width2, table_height2)) #add by chen #self.scene.add_box("table3", table_pose3, (table_depth3, table_width3, table_height3)) # add by chen # # We need to wait for the object part to appear self.wait_for_planning_scene_object() #########self.wait_for_planning_scene_object("table") #self.wait_for_planning_scene_object("table1") #self.wait_for_planning_scene_object("table2") #self.wait_for_planning_scene_object("table3") # 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") rospy.sleep(2.0) # sleep a little while 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("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") #self.scene.remove_world_object("table1") #self.scene.remove_world_object("table2") #self.scene.remove_world_object("table3") rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) # Removing is fast #rospy.loginfo("Adding new 'part2' object") rospy.loginfo("Object pose: %s", object_pose.pose) # Add object description in scene #self.scene.add_box("part2", object_pose, (self.object_depth, self.object_width, self.object_height)) rospy.loginfo("Second%s", object_pose.pose) table_pose = copy.deepcopy(object_pose) # define a virtual table below the object table_height = object_pose.pose.position.z - self.object_height / 2 table_width = 1.2 table_depth = 1.2 table_pose.pose.position.z += -( self.object_height) / 2 - table_height / 2 table_height -= 0.02 # 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)) #self.wait_for_planning_scene_object("part2") ####self.wait_for_planning_scene_object("table") 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
class Manager(object): ALL = "ALL" def __init__(self, name): rospy.loginfo("Starting node: %s" % name) config_file = rospy.get_param("~config_file", '') if config_file == '': rospy.logfatal("No config file specified") raise rospy.ROSException("No config file specified") return self.config = rosparam.load_file(config_file)[0][0] if self.ALL in self.config.keys(): self.all = self.config[self.ALL]["events"] else: self.all = [] print self.config rospy.loginfo("Creating mary client") self.mary_client = SimpleActionClient("speak", maryttsAction) self.mary_client.wait_for_server() rospy.loginfo(" ... done") rospy.Subscriber("/task_executor/events", TaskEvent, callback=self.callback) def callback(self, msg): if msg.task.action[1:] in self.config.keys(): events = itertools.chain( self.all, self.config[msg.task.action[1:]]["events"]) else: events = self.all for event in events: execute = True if msg.event == eval("TaskEvent." + event.keys()[0]): if "compare" in event.values()[0].keys(): comps = event.values()[0]["compare"] for comp in comps: if not eval( str(comp["static_value"]) + comp["comparison"] + "msg." + comp["task_field"]): execute = False break if "compare_to_topic" in event.values()[0].keys(): comps = event.values()[0]["compare_to_topic"] for comp in comps: res = rospy.wait_for_message( comp["topic"], roslib.message.get_message_class( rostopic.get_topic_type(comp["topic"], True)[0])) if not eval("res." + comp["field"] + comp["comparison"] + "msg." + comp["task_field"]): execute = False break if execute: self.mary_client.send_goal_and_wait( maryttsGoal(text=event.values()[0]["text"])) rospy.loginfo("Saying: " + event.values()[0]["text"])
StateMachine.add('close_goal', SimpleActionState(topic, MoveBaseAction, goal_cb=close_goal_cb), transitions={'succeeded': 'move_base', 'aborted': 'close_goal', 'preempted': 'close_goal'}) return sm if __name__ == '__main__': rospy.init_node('move_base_recovery') topic = rospy.get_param('~topic') base_frame = rospy.get_param('~base_frame') move_base = SimpleActionClient(topic, MoveBaseAction) move_base.wait_for_server() sm_recovery = create_machine(topic, base_frame) recovery_server = ActionServerWrapper( topic + '_recovery', MoveBaseAction, sm_recovery, ['succeeded'], ['aborted'], ['preempted'], goal_key='goal_message') recovery_server.run_server() rospy.spin()
class Manipulator: def __init__(self): self.arm = moveit_commander.MoveGroupCommander('arm') self.hand = moveit_commander.MoveGroupCommander('gripper') # self.group.set_planning_time(10) self.arm.set_pose_reference_frame('manip_base_link') # self.hand.set_planning_time(10) self.hand.set_pose_reference_frame('manip_base_link') self.__hand_traj_client = SimpleActionClient( "/gripper_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(60.0)) is False: rospy.logfatal( "Failed to connect to /gripper_controller/follow_joint_trajectory in 4sec." ) raise Exception( "Failed to connect to /gripper_controller/follow_joint_trajectory in 4sec." ) if self.__arm_traj_client.wait_for_server( timeout=rospy.Duration(60.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.joint_trajectory = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10) self.update_start_state = rospy.Publisher( '/rviz/moveit/update_start_state', Empty, queue_size=10) rospy.wait_for_service('/get_planning_scene', 60.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) rospy.Service('manipulator', setGoal, self.handler) self.tf = tf.TransformListener() self.tf.waitForTransform('manip_base_link', 'base', rospy.Time(), rospy.Duration(1.0)) self.is_moving = False rospy.loginfo('[manip] Going Home in 5 seconds...') self.home() def display_planned_path_callback(self, data): self.plan = data.trajectory[0].joint_trajectory def execute(self): self.joint_trajectory.publish(self.plan) self.is_moving = True def feedback_callback(self, data): self.is_moving = False if self.update_start_state.get_num_connections() > 0: self.update_start_state.publish() def execute_plan(self): self.arm.plan() rospy.sleep(2) if self.plan != None: self.execute() while self.is_moving: pass rospy.sleep(2) self.plan = None return True else: return False def execute_cartesian_plan(self, waypoints): plan, fraction = self.arm.compute_cartesian_path(waypoints, 0.001, 0, avoid_collisions=True) rospy.loginfo('[manip] Cartesian path fraction: %.2f.' % fraction) rospy.sleep(2) if self.plan != None and fraction > 0.9: self.arm.execute(plan, wait=True) rospy.sleep(5) self.plan = None return True else: self.plan = None return False def advance(self, waypoints, avoid_col=False): plan, fraction = self.arm.compute_cartesian_path( waypoints, 0.001, 0, avoid_collisions=avoid_col) rospy.loginfo('[manip] Cartesian path fraction: %.2f.' % fraction) rospy.sleep(2) if self.plan != None and fraction > 0.9: self.execute() while self.is_moving: pass rospy.sleep(5) self.plan = None return True else: self.plan = None return False def reset_manipulator(self): self.arm.set_named_target('home') plan = self.arm.plan() if not self.arm.execute(plan, wait=True): return False self.open_gripper() return True def home(self): self.arm.set_named_target('home') plan = self.arm.plan() if not self.arm.execute(plan, wait=True): return False return True def front(self): self.arm.set_named_target('front') plan = self.arm.plan() if not self.arm.execute(plan, wait=True): return False return True def attack(self): self.arm.set_named_target('attack') plan = self.arm.plan() if not self.arm.execute(plan, wait=True): return False return True def open_gripper(self): self.hand.set_named_target('open_gripper') plan = self.hand.plan() if not self.hand.execute(plan, wait=True): return False return True def close_gripper(self): self.hand.set_named_target('close_gripper') plan = self.hand.plan() success = self.hand.execute(plan, wait=True) return success def handler(self, request): type = request.type.lower() goal = request.goal pose = Pose() quaternion = tf.transformations.quaternion_from_euler( goal.rx, goal.ry, goal.rz) pose.position.x = goal.x pose.position.y = goal.y pose.position.z = goal.z pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] if type == 'reset': success = self.reset_manipulator() elif type == 'home': success = self.home() elif type == 'open': success = self.open_gripper() elif type == 'old_close': success = self.close_gripper() elif type == 'front': success = self.front() elif type == 'attack': success = self.attack() elif type == '': target_pose = copy.copy(pose) self.arm.set_pose_target(target_pose) plan = self.arm.plan() success = self.arm.execute(plan, wait=True) if success: return 'SUCCEEDED' else: return 'FAILED' elif type == 'pick': target_pose = copy.deepcopy(pose) self.arm.set_pose_target(target_pose) plan = self.arm.plan() success = self.arm.execute(plan, wait=True) rospy.sleep(10) self.close_gripper() rospy.sleep(5) self.home() if success: return 'SUCCEEDED' else: return 'FAILED' elif type == 'place': target_pose = copy.deepcopy(pose) self.arm.set_pose_target(target_pose) plan = self.arm.plan() success = self.arm.execute(plan, wait=True) rospy.sleep(5) self.open_gripper() rospy.sleep(10) self.home() elif type == 'point': self.close_gripper() joint_goal = self.arm.get_current_joint_values() joint_goal[0] = 0.35 joint_goal[1] = 0.0 joint_goal[2] = 0.0 joint_goal[3] = -2.35 joint_goal[4] = 0.0 joint_goal[5] = -0.35 self.arm.set_joint_value_target(joint_goal) plan = self.arm.plan() success = self.arm.execute(plan, wait=True) if success: return 'SUCCEEDED' else: return 'FAILED'
def test_grasp_action(self): """Test sending a grasp.""" goal = GraspGoal() goal.grasp = self.mk_grasp({ 'LFJ3': 1.4, 'RFJ3': 1.4, 'MFJ3': 1.4, 'FFJ3': 1.4, 'LFJ0': 3.0, 'RFJ0': 3.0, 'MFJ0': 3.0, 'FFJ0': 3.0, }) # Get a action client client = SimpleActionClient('grasp', GraspAction) client.wait_for_server() # Send pre-grasp goal.pre_grasp = True client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(20.0)) self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED, "Action did not return in SUCCEEDED state.") rospy.sleep(2) # Send grasp goal.pre_grasp = False client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(20.0)) self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED, "Action did not return in SUCCEEDED state.")
class Arm(object): CART_SERVER = '/arm_controller/move_arm_cart' def __init__(self, default_pitch=(math.pi / 2.0)): rospy.loginfo('Initializing arm control.') rospy.loginfo('Waiting for [%s] server...' % (self.CART_SERVER)) self.move_arm_cart_server = SimpleActionClient(self.CART_SERVER, MoveArmAction) self.move_arm_cart_server.wait_for_server() self.script_server = simple_script_server() self.pitch = default_pitch def move_to(self, where): if isinstance(where, str): self._move_to_pose(where) elif len(where) == 5: self._move_to_joints(where) elif len(where) == 4 or len(where) == 3: self._move_to_cartesian(where) def _move_to_joints(self, joints): self.script_server.move('arm', [joints]) def _move_to_cartesian(self, coordinates): """ Move the arm to the pose given by (x, y, z, pitch) tuple. The pitch is optional and can be omitted. """ g = MoveArmGoal() pc = PositionConstraint() pc.header.frame_id = "/base_link" pc.header.stamp = rospy.Time.now() pc.position.x = coordinates[0] pc.position.y = coordinates[1] pc.position.z = coordinates[2] g.motion_plan_request.goal_constraints.position_constraints.append(pc) oc = OrientationConstraint() r = 0.0 p = coordinates[3] if len(coordinates) == 4 else self.pitch y = 0.0 (qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(r, p, y) oc.header.frame_id = "/base_link" oc.header.stamp = rospy.Time.now() oc.orientation.x = qx oc.orientation.y = qy oc.orientation.z = qz oc.orientation.w = qw g.motion_plan_request.goal_constraints.orientation_constraints.append( oc) self.move_arm_cart_server.send_goal(g) rospy.loginfo('Sent move arm goal, waiting for result...') self.move_arm_cart_server.wait_for_result() rv = self.move_arm_cart_server.get_result().error_code.val print rv if not rv == 1: raise Exception('Failed to move the arm to the given pose.') def _move_to_pose(self, pose): self.script_server.move('arm', pose) def open_gripper(self): self.script_server.move('gripper', 'open') def close_gripper(self): self.script_server.move('gripper', 'close')
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 StateMachine(object): def __init__(self): self.node_name = "Student SM" # Access rosparams self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic') self.mv_head_srv_nm = rospy.get_param(rospy.get_name() + '/move_head_srv') # Subscribe to topics # Wait for service providers rospy.wait_for_service(self.mv_head_srv_nm, timeout=30) # Instantiate publishers self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top, Twist, queue_size=10) # Set up action clients rospy.loginfo("%s: Waiting for play_motion action server...", self.node_name) self.play_motion_ac = SimpleActionClient("/play_motion", PlayMotionAction) if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)): rospy.logerr("%s: Could not connect to /play_motion action server", self.node_name) exit() rospy.loginfo("%s: Connected to play_motion action server", self.node_name) # Init state machine self.state = 0 rospy.sleep(3) self.check_states() def check_states(self): while not rospy.is_shutdown() and self.state != 4: # State 0: Move the robot "manually" to door if self.state == 0: move_msg = Twist() move_msg.linear.x = 1 rate = rospy.Rate(10) converged = False cnt = 0 rospy.loginfo("%s: Moving towards door", self.node_name) while not rospy.is_shutdown() and cnt < 25: self.cmd_vel_pub.publish(move_msg) rate.sleep() cnt = cnt + 1 self.state = 1 rospy.sleep(1) # State 1: Tuck arm if self.state == 1: rospy.loginfo("%s: Tucking the arm...", self.node_name) goal = PlayMotionGoal() goal.motion_name = 'home' goal.skip_planning = True self.play_motion_ac.send_goal(goal) success_tucking = self.play_motion_ac.wait_for_result(rospy.Duration(100.0)) if success_tucking: rospy.loginfo("%s: Arm tuck: ", self.play_motion_ac.get_result()) self.state = 2 else: self.play_motion_ac.cancel_goal() rospy.logerr("%s: play_motion failed to tuck arm, reset simulation", self.node_name) self.state = 5 rospy.sleep(1) # State 2: Move the robot "manually" to chair if self.state == 2: move_msg = Twist() move_msg.angular.z = -1 rate = rospy.Rate(10) converged = False cnt = 0 rospy.loginfo("%s: Moving towards table", self.node_name) while not rospy.is_shutdown() and cnt < 5: self.cmd_vel_pub.publish(move_msg) rate.sleep() cnt = cnt + 1 move_msg.linear.x = 1 move_msg.angular.z = 0 cnt = 0 while not rospy.is_shutdown() and cnt < 15: self.cmd_vel_pub.publish(move_msg) rate.sleep() cnt = cnt + 1 self.state = 3 rospy.sleep(1) # State 3: Lower robot head service if self.state == 3: try: rospy.loginfo("%s: Lowering robot head", self.node_name) move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm, MoveHead) move_head_req = move_head_srv("down") if move_head_req.success == True: self.state = 4 rospy.loginfo("%s: Move head down succeded!", self.node_name) else: rospy.loginfo("%s: Move head down failed!", self.node_name) self.state = 5 rospy.sleep(3) except rospy.ServiceException, e: print "Service call to move_head server failed: %s"%e # Error handling if self.state == 5: rospy.logerr("%s: State machine failed. Check your code and try again!", self.node_name) return rospy.loginfo("%s: State machine finished!", self.node_name) return
class 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 Robot: '''Robot class that provides an interface to arm and head (maybe base later)''' def __init__(self, tf_listener): ''' Args: tf_listener (TransformListener) ''' self._social_gaze = rospy.get_param("/social_gaze") self._play_sound = rospy.get_param("/play_sound") self._tf_listener = tf_listener # arm services self.move_arm_to_joints_plan_srv = rospy.ServiceProxy( 'move_arm_to_joints_plan', MoveArm) rospy.wait_for_service('move_arm_to_joints_plan') self.move_arm_to_joints_srv = rospy.ServiceProxy( 'move_arm_to_joints', MoveArmTraj) rospy.wait_for_service('move_arm_to_joints') self.move_arm_to_pose_srv = rospy.ServiceProxy('move_arm_to_pose', MoveArm) rospy.wait_for_service('move_arm_to_pose') self.start_move_arm_to_pose_srv = rospy.ServiceProxy( 'start_move_arm_to_pose', MoveArm) rospy.wait_for_service('start_move_arm_to_pose') self.is_reachable_srv = rospy.ServiceProxy('is_reachable', MoveArm) rospy.wait_for_service('is_reachable') self.is_arm_moving_srv = rospy.ServiceProxy('is_arm_moving', GetArmMovement) rospy.wait_for_service('is_arm_moving') self.relax_arm_srv = rospy.ServiceProxy('relax_arm', Empty) rospy.wait_for_service('relax_arm') self.reset_arm_movement_history_srv = rospy.ServiceProxy( 'reset_arm_movement_history', Empty) rospy.wait_for_service('reset_arm_movement_history') self.get_gripper_state_srv = rospy.ServiceProxy( 'get_gripper_state', GetGripperState) rospy.wait_for_service('get_gripper_state') self.get_joint_states_srv = rospy.ServiceProxy('get_joint_states', GetJointStates) rospy.wait_for_service('get_joint_states') self.get_ee_pose_srv = rospy.ServiceProxy('get_ee_pose', GetEEPose) rospy.wait_for_service('get_ee_pose') self.set_gripper_state_srv = rospy.ServiceProxy( 'set_gripper_state', SetGripperState) rospy.wait_for_service('set_gripper_state') rospy.loginfo("Got all arm services") # head services self.gaze_client = SimpleActionClient('gaze_action', GazeAction) self.gaze_client.wait_for_server(rospy.Duration(5)) self.current_gaze_goal_srv = rospy.ServiceProxy( 'get_current_gaze_goal', GetGazeGoal) rospy.wait_for_service('get_current_gaze_goal') rospy.loginfo("Got all head services") # world services self._get_nearest_object_srv = rospy.ServiceProxy( 'get_nearest_object', GetNearestObject) rospy.wait_for_service('get_nearest_object') rospy.loginfo("Got all world services") # sound stuff self._sound_client = SoundClient() # arm stuff def move_arm_to_pose(self, arm_state): '''Move robot's arm to pose Args: arm_state (ArmState) : contains pose info Return: bool : success ''' try: result = self.move_arm_to_pose_srv(arm_state).success except Exception, e: result = False return result
class NearestWLANFinderClient(object): """A test for osm_bridge_ros.py 's nearest_wlan_finder action""" def __init__(self): rospy.loginfo("inside __init__ of NearestWLANFinderClient") SERVER = "/nearest_wlan" self.client = SimpleActionClient(SERVER, NearestWLANAction) self.client.wait_for_server() rospy.loginfo("Connected to server") req = NearestWLANGoal(x=0.0, y=5.0, is_x_y_provided=True, floor='BRSU_L0') self.client.send_goal(req, done_cb=self.done_cb) self.client.wait_for_result() req = NearestWLANGoal(x=0.0, y=5.0, is_x_y_provided=True, floor='127') self.client.send_goal(req, done_cb=self.done_cb) self.client.wait_for_result() req = NearestWLANGoal(is_x_y_provided=False, area='BRSU_A_L0_A1') self.client.send_goal(req, done_cb=self.done_cb) self.client.wait_for_result() req = NearestWLANGoal(is_x_y_provided=False, area='125') self.client.send_goal(req, done_cb=self.done_cb) self.client.wait_for_result() req = NearestWLANGoal(is_x_y_provided=False, local_area='BRSU_A_L0_A1_LA1') self.client.send_goal(req, done_cb=self.done_cb) self.client.wait_for_result() req = NearestWLANGoal(is_x_y_provided=False, local_area='120') self.client.send_goal(req, done_cb=self.done_cb) self.client.wait_for_result() rospy.signal_shutdown("Test complete") def done_cb(self, status, result): # rospy.loginfo(status) # rospy.loginfo(result) try: assert (result.point.id == 2658) rospy.loginfo("Test passed") except Exception as e: rospy.logerr("Test failed")
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 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, -1.0] jtp.time_from_start = rospy.Duration(2.0) jt.points.append(jtp) self.head_cmd.publish(jt) rospy.loginfo("Done.") def lower_torso(self): rospy.loginfo("Moving torso down") jt = JointTrajectory() jt.joint_names = ['torso_lift_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0.00] jtp.time_from_start = rospy.Duration(2.5) jt.points.append(jtp) self.torso_cmd.publish(jt) def pick_aruco(self, aruco_pose): # rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection") # aruco_pose = PoseStamped() # aruco_pose.header.frame_id = "base_footprint" # aruco_pose.pose.position.x = 0.492726171711 # aruco_pose.pose.position.y = -0.140456914268 # aruco_pose.pose.position.z = 0.133627714861 # aruco_pose.pose.orientation.x = 0.544020678534 # aruco_pose.pose.orientation.y = 0.507215268602 # aruco_pose.pose.orientation.z = 0.500950386556 # aruco_pose.pose.orientation.w = 0.442518793765 # aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped) aruco_pose.header.frame_id = self.strip_leading_slash( aruco_pose.header.frame_id) rospy.loginfo("Got: " + str(aruco_pose)) self.prepare_robot() rospy.sleep(2.0) # rospy.loginfo("spherical_grasp_gui: Transforming from frame: {} to 'base_footprint'".format(aruco_pose.header.frame_id)) # 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() pick_g = PickUpPoseGoal() aruco_ps = aruco_pose # 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!") result = self.pick_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr( "Failed to pick, not trying further - error code was {}". format(str(moveit_error_dict[result.error_code]))) # return # move to final position 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.") # Move torso to its maximum height # self.lift_torso() #self.lower_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 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 unfolding arm.") self.lower_torso() self.lower_head() rospy.loginfo("Robot prepared.")
client.send_goal(goal) client.wait_for_result() if client.get_state() == GoalStatus.SUCCEEDED: result = client.get_result() print "Result: " + str(result.base_position) elif client.get_state() == GoalStatus.PREEMPTED: print "Action pre-empted" else: print "Action failed" if __name__ == '__main__': try: rospy.init_node(NAME, anonymous=True) client = SimpleActionClient("erratic_base_action", ErraticBaseAction) client.wait_for_server() print "Go exactly 1 meter forward & 1 meter to the right..." position = Point(x=1.0, y=-1.0) orientation = Quaternion(w=1.0) move_to('/base_footprint', position, orientation) # No vicinity specified rospy.sleep(2.0) print "Go to rocky sphere..." position = Point(x=-2.6, y=1.7) orientation = Quaternion(w=1.0) move_to('/map', position, orientation, 0.9) rospy.sleep(2.0)
class CokeCanPickAndPlace: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'coke_can') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.06) 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.6) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.4) # 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) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_coke_can(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: 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.5 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)) rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x, self._pose_place.position.y, self._pose_place.position.z) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) #self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) 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 = 1.0 p.pose.position.y = 0.0 p.pose.position.z = 1.0 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.5, 0.8, 0.03)) return p.pose def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.75 - 0.01 p.pose.position.y = 0.25 - 0.01 p.pose.position.z = 1.00 + (0.3 + 0.03) / 2.0 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.15, 0.15, 0.3)) 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 = 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) # 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 = -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 = 1 # 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 = 5.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 = 10 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 = 5.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 = 10 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)
#! /usr/bin/env python import sys import rospy from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal from diagnostic_msgs.msg import KeyValue if __name__ == '__main__': rospy.init_node('pick_object_client_tester') client = SimpleActionClient('wbc_pick_object_server', GenericExecuteAction) client.wait_for_server() if len(sys.argv) == 3: goal = GenericExecuteGoal() obj = str(sys.argv[1]).upper() location = str(sys.argv[2]).upper() goal.parameters.append(KeyValue(key='object', value=obj)) goal.parameters.append(KeyValue(key='location', value=location)) rospy.loginfo('Sending following goal to place object server') rospy.loginfo(goal) client.send_goal(goal) timeout = 15.0 finished_within_time = client.wait_for_result( rospy.Duration.from_sec(int(timeout))) if not finished_within_time:
def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'coke_can') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.06) 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.6) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.4) # 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) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_coke_can(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: 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.5 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)) rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x, self._pose_place.position.y, self._pose_place.position.z) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) #self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) 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')
class PathExecutor: _feedback = ExecutePathFeedback() _result = ExecutePathResult() def __init__(self, name): # self._goal_publisher = Simple # self.pe_client_for_bug2= SimpleActionClient('/bug2/move_to',MoveToAction) # self.pe_client_for_bug2.wait_for_server() print "----------------- in INIT ---------------------------------------" rospy.loginfo("Creating an action server") # Creating a client to perform the bug2 function self._move_to_client = SimpleActionClient('/bug2/move_to', MoveToAction) # Creating an action server self._as = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, \ execute_cb=self.execute_cb, auto_start=False) # Starting the server self._as.start() # Using a flag to aid preemption self.flag = True # Using a flag to detect the completion of 3 goals self.success = True def execute_cb(self, goal): # Executed every time a goal is sent by the client self._move_to_client.wait_for_server() # printing the goals in the terminal rospy.loginfo(goal.path.poses) # Creating an instance of MoveToGoal to compose a goal message self.move = MoveToGoal() # Iterating through the goals and sending it to the move_to_client for i in range(len(goal.path.poses)): if i == 2: self.success = True else: self.success = False self._pose = goal.path.poses[i] self.move.target_pose = self._pose self._move_to_client.send_goal(self.move, done_cb=self.move_to_done_cb) self._move_to_client.wait_for_result() # self.success = True rospy.spin() # setting the server's preempted() function if it's requested while flag: if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() break def move_to_done_cb(self, state, result): if (state == 3): # self._result.visited.append(True) self._feedback.reached = True self._feedback.pose = self._pose elif (state == 4): self._feedback.reached = False # rospy.loginfo(self._result) self._as.publish_feedback(self._feedback) if self.success == True: print "All goals are considered" self._as.set_succeeded(self._result)
def __init__(self): rospy.wait_for_service('get_exploration_path', timeout=2) self._plan_service = rospy.ServiceProxy('get_exploration_path', GetRobotTrajectory) self._move_base = SimpleActionClient('move_base', MoveBaseAction)
class GiskardWrapper(object): def __init__(self, giskard_topic=u'giskardpy/command', ns=u'giskard'): if giskard_topic is not None: self.client = SimpleActionClient(giskard_topic, MoveAction) self.update_world = rospy.ServiceProxy(u'{}/update_world'.format(ns), UpdateWorld) self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10) rospy.wait_for_service(u'{}/update_world'.format(ns)) self.client.wait_for_server() self.tip_to_root = {} self.robot_urdf = URDFObject(rospy.get_param(u'robot_description')) self.collisions = [] self.clear_cmds() self.object_js_topics = {} rospy.sleep(.3) def get_robot_name(self): return self.robot_urdf.get_name() def get_root(self): return self.robot_urdf.get_root() def set_cart_goal(self, root, tip, pose_stamped, trans_max_speed=None, rot_max_speed=None): """ :param tip: :type tip: str :param pose_stamped: :type pose_stamped: PoseStamped """ self.set_translation_goal(root, tip, pose_stamped, max_speed=trans_max_speed) self.set_rotation_goal(root, tip, pose_stamped, max_speed=rot_max_speed) def set_translation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None): """ :param tip: :type tip: str :param pose_stamped: :type pose_stamped: PoseStamped """ if not gain and not max_speed and not weight: constraint = CartesianConstraint() constraint.type = CartesianConstraint.TRANSLATION_3D constraint.root_link = str(root) constraint.tip_link = str(tip) constraint.goal = pose_stamped self.cmd_seq[-1].cartesian_constraints.append(constraint) else: constraint = Constraint() constraint.type = u'CartesianPosition' params = {} params[u'root_link'] = root params[u'tip_link'] = tip params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped) if gain: params[u'gain'] = gain if max_speed: params[u'max_speed'] = max_speed if weight: params[u'weight'] = weight constraint.parameter_value_pair = json.dumps(params) self.cmd_seq[-1].constraints.append(constraint) def set_rotation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None): """ :param tip: :type tip: str :param pose_stamped: :type pose_stamped: PoseStamped """ if not gain and not max_speed and not weight: constraint = CartesianConstraint() constraint.type = CartesianConstraint.ROTATION_3D constraint.root_link = str(root) constraint.tip_link = str(tip) constraint.goal = pose_stamped self.cmd_seq[-1].cartesian_constraints.append(constraint) else: constraint = Constraint() constraint.type = u'CartesianOrientationSlerp' params = {} params[u'root_link'] = root params[u'tip_link'] = tip params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped) if gain: params[u'gain'] = gain if max_speed: params[u'max_speed'] = max_speed if weight: params[u'weight'] = weight constraint.parameter_value_pair = json.dumps(params) self.cmd_seq[-1].constraints.append(constraint) def set_joint_goal(self, joint_state, weight=None, gain=None, max_speed=None): """ :param joint_state: :type joint_state: dict """ if not weight and not gain and not max_speed: constraint = JointConstraint() constraint.type = JointConstraint.JOINT if isinstance(joint_state, dict): for joint_name, joint_position in joint_state.items(): constraint.goal_state.name.append(joint_name) constraint.goal_state.position.append(joint_position) elif isinstance(joint_state, JointState): constraint.goal_state = joint_state self.cmd_seq[-1].joint_constraints.append(constraint) elif isinstance(joint_state, dict): for joint_name, joint_position in joint_state.items(): constraint = Constraint() constraint.type = u'JointPosition' params = {} params[u'joint_name'] = joint_name params[u'goal'] = joint_position if weight: params[u'weight'] = weight if gain: params[u'gain'] = gain if max_speed: params[u'max_speed'] = max_speed constraint.parameter_value_pair = json.dumps(params) self.cmd_seq[-1].constraints.append(constraint) def align_planes(self, tip, tip_normal, root=None, root_normal=None): """ :type tip: str :type tip_normal: Vector3Stamped :type root: str :type root_normal: Vector3Stamped :return: """ root = root if root else self.get_root() tip_normal = convert_ros_message_to_dictionary(tip_normal) if not root_normal: root_normal = Vector3Stamped() root_normal.header.frame_id = self.get_root() root_normal.vector.z = 1 root_normal = convert_ros_message_to_dictionary(root_normal) self.set_json_goal(u'AlignPlanes', tip=tip, tip_normal=tip_normal, root=root, root_normal=root_normal) def gravity_controlled_joint(self, joint_name, object_name): self.set_json_goal(u'GravityJoint', joint_name=joint_name, object_name=object_name) def update_god_map(self, updates): self.set_json_goal(u'UpdateGodMap', updates=updates) def update_yaml(self, updates): self.set_json_goal(u'UpdateYaml', updates=updates) def pointing(self, tip, goal_point, root=None, pointing_axis=None, weight=None): kwargs = {u'tip':tip, u'goal_point':goal_point} if root is not None: kwargs[u'root'] = root if pointing_axis is not None: kwargs[u'pointing_axis'] = convert_ros_message_to_dictionary(pointing_axis) if weight is not None: kwargs[u'weight'] = weight kwargs[u'goal_point'] = convert_ros_message_to_dictionary(goal_point) self.set_json_goal(u'Pointing', **kwargs) def set_json_goal(self, constraint_type, **kwargs): constraint = Constraint() constraint.type = constraint_type for k, v in kwargs.items(): if isinstance(v, Message): kwargs[k] = convert_ros_message_to_dictionary(v) constraint.parameter_value_pair = json.dumps(kwargs) self.cmd_seq[-1].constraints.append(constraint) def set_collision_entries(self, collisions): self.cmd_seq[-1].collisions.extend(collisions) def allow_collision(self, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL, link_bs=(CollisionEntry.ALL,)): """ :param robot_links: list of robot link names as str, None or empty list means all :type robot_links: list :param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies :type body_b: str :param link_bs: list of link name of body_b, None or empty list means all :type link_bs: list """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [str(x) for x in robot_links] collision_entry.body_b = str(body_b) collision_entry.link_bs = [str(x) for x in link_bs] self.set_collision_entries([collision_entry]) def avoid_collision(self, min_dist, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL, link_bs=(CollisionEntry.ALL,)): """ :param min_dist: the distance giskard is trying to keep between specified links :type min_dist: float :param robot_links: list of robot link names as str, None or empty list means all :type robot_links: list :param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies :type body_b: str :param link_bs: list of link name of body_b, None or empty list means all :type link_bs: list """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = min_dist collision_entry.robot_links = [str(x) for x in robot_links] collision_entry.body_b = str(body_b) collision_entry.link_bs = [str(x) for x in link_bs] self.set_collision_entries([collision_entry]) def allow_all_collisions(self): """ Allows all collisions for next goal. """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = CollisionEntry.ALL collision_entry.link_bs = [CollisionEntry.ALL] self.set_collision_entries([collision_entry]) def allow_self_collision(self): collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = self.get_robot_name() collision_entry.link_bs = [CollisionEntry.ALL] self.set_collision_entries([collision_entry]) def set_self_collision_distance(self, min_dist=0.05): collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = self.get_robot_name() collision_entry.link_bs = [CollisionEntry.ALL] collision_entry.min_dist = min_dist self.set_collision_entries([collision_entry]) def avoid_all_collisions(self, distance=0.05): """ Avoids all collisions for next goal. :param distance: the distance that giskard is trying to keep from all objects :type distance: float """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = CollisionEntry.ALL collision_entry.link_bs = [CollisionEntry.ALL] collision_entry.min_dist = distance self.set_collision_entries([collision_entry]) def add_cmd(self, max_trajectory_length=20): move_cmd = MoveCmd() # move_cmd.max_trajectory_length = max_trajectory_length self.cmd_seq.append(move_cmd) def clear_cmds(self): """ Removes all move commands from the current goal, collision entries are left untouched. """ self.cmd_seq = [] self.add_cmd() def plan_and_execute(self, wait=True): """ :param wait: this function block if wait=True :type wait: bool :return: result from giskard :rtype: MoveResult """ goal = self._get_goal() if wait: self.client.send_goal_and_wait(goal) return self.client.get_result() else: self.client.send_goal(goal) def plan(self, wait=True): """ :param wait: this function block if wait=True :type wait: bool :return: result from giskard :rtype: MoveResult """ goal = self._get_goal() goal.type = MoveGoal.PLAN_ONLY if wait: self.client.send_goal_and_wait(goal) return self.client.get_result() else: self.client.send_goal(goal) def get_collision_entries(self): return self.cmd_seq def _get_goal(self): goal = MoveGoal() goal.cmd_seq = self.cmd_seq goal.type = MoveGoal.PLAN_AND_EXECUTE self.clear_cmds() return goal def interrupt(self): self.client.cancel_goal() def get_result(self, timeout=rospy.Duration()): """ Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False :type timeout: rospy.Duration :rtype: MoveResult """ self.client.wait_for_result(timeout) return self.client.get_result() def clear_world(self): """ :rtype: UpdateWorldResponse """ req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped()) return self.update_world.call(req) def remove_object(self, name): """ :param name: :type name: str :return: :rtype: UpdateWorldResponse """ object = WorldBody() object.name = str(name) req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False, PoseStamped()) return self.update_world.call(req) def add_box(self, name=u'box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): """ :param name: :param size: (x length, y length, z length) :param frame_id: :param position: :param orientation: :param pose: :return: """ box = make_world_body_box(name, size[0], size[1], size[2]) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose) return self.update_world.call(req) def add_sphere(self, name=u'sphere', size=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): object = WorldBody() object.type = WorldBody.PRIMITIVE_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.shape.type = SolidPrimitive.SPHERE object.shape.dimensions.append(size) req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self.update_world.call(req) def add_mesh(self, name=u'mesh', mesh=u'', frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): object = WorldBody() object.type = WorldBody.MESH_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.mesh = mesh req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self.update_world.call(req) def add_cylinder(self, name=u'cylinder', size=(1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): object = WorldBody() object.type = WorldBody.PRIMITIVE_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.shape.type = SolidPrimitive.CYLINDER object.shape.dimensions.append(size[0]) object.shape.dimensions.append(size[1]) req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self.update_world.call(req) def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None): """ :type name: str :type size: list :type frame_id: str :type position: list :type orientation: list :rtype: UpdateWorldResponse """ box = make_world_body_box(name, size[0], size[1], size[2]) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) if frame_id is not None else u'map' pose.pose.position = Point(*(position if position is not None else [0, 0, 0])) pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1])) req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose) return self.update_world.call(req) def attach_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=None, position=None, orientation=None): """ :type name: str :type size: list :type frame_id: str :type position: list :type orientation: list :rtype: UpdateWorldResponse """ cylinder = make_world_body_cylinder(name, height, radius) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) if frame_id is not None else u'map' pose.pose.position = Point(*(position if position is not None else [0, 0, 0])) pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1])) req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose) return self.update_world.call(req) def attach_object(self, name, link_frame_id): """ :type name: str :type link_frame_id: str :return: UpdateWorldResponse """ req = UpdateWorldRequest() req.rigidly_attached = True req.body.name = name req.pose.header.frame_id = link_frame_id req.operation = UpdateWorldRequest.ADD return self.update_world.call(req) def detach_object(self, object_name): req = UpdateWorldRequest() req.body.name = object_name req.operation = req.DETACH return self.update_world.call(req) def add_urdf(self, name, urdf, pose, js_topic=u''): urdf_body = WorldBody() urdf_body.name = str(name) urdf_body.type = WorldBody.URDF_BODY urdf_body.urdf = str(urdf) urdf_body.joint_state_topic = str(js_topic) req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False, pose) if js_topic: # FIXME publisher has to be removed, when object gets deleted # FIXME there could be sync error, if objects get added/removed by something else self.object_js_topics[name] = rospy.Publisher(js_topic, JointState, queue_size=10) return self.update_world.call(req) def set_object_joint_state(self, object_name, joint_states): if isinstance(joint_states, dict): joint_states = dict_to_joint_states(joint_states) self.object_js_topics[object_name].publish(joint_states)
class 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.") self.pick_g = None self.move_pub = rospy.Publisher('/mobile_base_controller/cmd_vel', Twist, queue_size=1) self.scene = PlanningSceneInterface() def strip_leading_slash(self, s): return s[1:] if s.startswith("/") else s def pick_aruco(self, string_operation): #rospy.sleep(2.0) if string_operation == "pick": self.prepare_robot() # Detect object rospy.loginfo( "spherical_grasp_gui: Waiting for an aruco detection") aruco_pose = rospy.wait_for_message('/aruco_single/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.pose.orientation = aruco_pose.pose.orientation 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() 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!") # 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.") # Move to safe home = False i = 0 #self.move_back(5) while not home and i < 3: home = self.move_arm_home() i += 1 rospy.sleep(0.5) result = self.pick_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr("Failed to pick, not trying further") return TriggerResponse(False, "Failed to pick") self.pick_g = pick_g return TriggerResponse(True, "Succeeded") elif string_operation == "place": if not self.pick_g: rospy.logerr("Failed to place, no object position known") return TriggerResponse( False, "Failed to place, no object position known") pick_g = self.pick_g # Move torso to its maximum height self.lift_torso() # Raise arm rospy.loginfo("Moving arm to a safe pose") pmg = PlayMotionGoal() pmg.motion_name = 'prepare_grasp' #'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.35 self.place_as.send_goal_and_wait(pick_g) rospy.loginfo("Done!") # Move to safe home = False i = 0 #self.move_back(5) while not home and i < 3: home = self.move_arm_home() i += 1 rospy.sleep(0.5) result = self.place_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr("Failed to place") return TriggerResponse(False, "Failed to place") self.pick_g = None return TriggerResponse(True, "Succeeded") 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.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.") def move_back(self, n): for i in range(n): self.move_pub.publish(Twist(linear=Vector3(-0.15, 0, 0))) rospy.sleep(0.05) def move_arm_home(self): rospy.loginfo("Fold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'home' pmg.skip_planning = False res = self.play_m_as.send_goal_and_wait(pmg) if res != GoalStatus.SUCCEEDED: return False self.scene.remove_world_object("table") rospy.loginfo("Done.") return True
marker = False moveit_commander.roscpp_initialize(sys.argv) scene = moveit_commander.PlanningSceneInterface() robot = moveit_commander.RobotCommander() interpreter = moveit_commander.MoveGroupCommandInterpreter() interpreter.execute("use arm") group = interpreter.get_active_group() eef_link=group.get_end_effector_link() scene.remove_attached_object(eef_link, name="pringles") scene.remove_world_object("pringles") scene.remove_world_object("table") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) rate = rospy.Rate(1) play_m_as = SimpleActionClient('/play_motion', PlayMotionAction) if not play_m_as.wait_for_server(rospy.Duration(20)): rospy.logerr("Could not connect to /play_motion AS") exit() rospy.loginfo("Connected!") head_cmd = rospy.Publisher( '/head_controller/command', JointTrajectory, queue_size=1) jt = JointTrajectory() jt.joint_names = ['head_1_joint', 'head_2_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0, -0.56] jtp.time_from_start = rospy.Duration(2.0) jt.points.append(jtp) head_cmd.publish(jt) init = datetime.now() #Detect object and determine its speed
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)