class Terminal(object): def __init__(self, name='terminal'): self.client = SimpleActionClient(resolve_name(name), ExchangeAction) self.client.wait_for_server() def __del__(self): self.close() def close(self): del self.client def send(self, mode, text, handler=None, block=True): goal = ExchangeGoal() goal.mode = mode goal.text = text self.client.send_goal(goal, feedback_cb=handler) if block: self.client.wait_for_result() def write(self, text): self.send(PRINT, text) def prompt(self, text): self.send(LINE, text) return self.client.get_result().line def get(self, text, escape, handler, block=True): callback = lambda feedback: handler(feedback.key) self.send(KEY, escape + text, callback, block)
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 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 IntroduceSelf(smach.State): def __init__(self, profession=True, residence=True, date_of_birth=True, timeout=120.0, action_server='/mdr_actions/introduce_self_action_server'): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.profession = profession self.residence = residence self.date_of_birth = date_of_birth self.timeout = timeout self.introduce_self_client = SimpleActionClient( action_server, IntroduceSelfAction) self.introduce_self_client.wait_for_server() def execute(self, userdata): goal = IntroduceSelfGoal() goal.profession = self.profession goal.residence = self.residence goal.date_of_birth = self.date_of_birth rospy.loginfo('Calling introduce_self action') self.introduce_self_client.send_goal(goal) duration = rospy.Duration.from_sec(self.timeout) self.introduce_self_client.wait_for_result(duration) rospy.loginfo('introduce_self call completed') res = self.introduce_self_client.get_result() if res and res.success: return 'succeeded' else: return 'failed'
class unstage_object(smach.State): def __init__(self, platform=None): smach.State.__init__(self, outcomes=['success', 'failed'], input_keys=['goal']) self.client = SimpleActionClient('unstage_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() if platform is not None: self.goal.parameters.append( KeyValue(key='platform', value=str(platform).upper())) def execute(self, userdata): # initialise platform in goal if not already initialised current_platform = Utils.get_value_of(self.goal.parameters, 'platform') if current_platform is None: platform = Utils.get_value_of(userdata.goal.parameters, 'platform') if platform is None: rospy.logwarn('Platform not provided. Using default') platform = 'platform_middle' self.goal.parameters.append( KeyValue(key='platform', value=str(platform).upper())) self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return 'success' else: return 'failed'
class Planning: def __init__(self): self.client = SimpleActionClient('/move_base', MoveBaseAction) self.goal = MoveBaseGoal() self.current_goal = 'home' self.goals = { 'home': np.array([0.0, 0.0]), 'pos1': np.array([1.0, 0.0]), 'pos2': np.array([1.0, 1.0]), } def sendGoal(self, x_pos, y_pos, yaw): if abs(x_pos) <= 0.1: x_pos = 0.0 if abs(y_pos) <= 0.1: y_pos = 0.0 # quaternion = transformations.quaternion_from_euler(0, 0, yaw) goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = x_pos goal.target_pose.pose.position.y = y_pos goal.target_pose.pose.position.z = 0.0 # goal.target_pose.pose.orientation.x = quaternion[0] # goal.target_pose.pose.orientation.y = quaternion[1] # goal.target_pose.pose.orientation.z = quaternion[2] goal.target_pose.pose.orientation.w = 1.0 #quaternion[3] self.client.send_goal(goal) self.client.wait_for_result() print('Reached ' + self.current_goal) print(self.client.get_result()) def computeNewGoal(self): if self.current_goal == 'pos2': self.current_goal = 'home' elif self.current_goal == 'pos1': self.current_goal = 'pos2' elif self.current_goal == 'home': self.current_goal = 'pos1' else: self.current_goal = 'home' self.sendGoal(self.goals[self.current_goal][0], self.goals[self.current_goal][1], 0) def loop(self, rate): self.sendGoal(self.goals['home'][0], self.goals['home'][1], 0) while not rospy.is_shutdown(): self.computeNewGoal() rate.sleep()
class Move(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['wp_reached', 'wp_not_reached', 'test_finished'], input_keys=['move_waypoints', 'move_target_wp', 'move_wp_str'], output_keys=['move_target_wp']) self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True) def execute(self, ud): self.move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose = ud.move_waypoints[ud.move_target_wp] goal.target_pose.header.stamp = rospy.Time.now() self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result() status = self.move_base_client.get_state() tts_msg = String() if status == GoalStatus.SUCCEEDED: tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "." self.tts_pub.publish(tts_msg) if ud.move_target_wp == 0: return 'test_finished' else: ud.move_target_wp -= 1 return 'succeeded' else: return 'wp_not_reached'
class ActionTasks: def __init__(self, script_path): rospy.init_node('action_testr') rospy.on_shutdown(self.cleanup) self.fridge = (Pose(Point(0, 0, 0.0), Quaternion(0.0, 0.0, 0, 1))) 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...")
class plan_task(smach.State): def __init__(self, mode=PlanGoal.NORMAL): smach.State.__init__( self, outcomes=['success', 'failure'], input_keys=['domain_file', 'problem_file', 'planner'], output_keys=['plan']) self.mode = mode self.planner_client = SimpleActionClient( '/mir_task_planning/task_planner_server/plan_task', PlanAction) self.planner_client.wait_for_server() rospy.sleep(0.2) def execute(self, userdata): goal = PlanGoal(domain_file=userdata.domain_file, problem_file=userdata.problem_file, planner=userdata.planner, mode=self.mode) self.planner_client.send_goal(goal) self.planner_client.wait_for_result() state = self.planner_client.get_state() result = self.planner_client.get_result() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Found a plan with %i actions", len(result.plan.plan)) userdata.plan = result.plan return 'success' elif state == GoalStatus.ABORTED: rospy.logerr("Failed to plan") rospy.sleep(2.0) return 'failure'
class EnterDoor(smach.State): def __init__(self, timeout=120.0, move_forward_server='/mdr_actions/move_forward_server', movement_duration=15., speed=0.1): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.timeout = timeout self.movement_duration = movement_duration self.speed = speed self.move_forward_client = SimpleActionClient(move_forward_server, MoveForwardAction) self.move_forward_client.wait_for_server() self.entered = False def execute(self, userdata): goal = MoveForwardGoal() goal.movement_duration = self.movement_duration goal.speed = self.speed self.move_forward_client.send_goal(goal) timeout_duration = rospy.Duration.from_sec(self.timeout) self.move_forward_client.wait_for_result(timeout_duration) result = self.move_forward_client.get_result() if result and result.success: return 'succeeded' else: return 'failed'
def _exec_action(action_client: SimpleActionClient, goal, stopping_event: EventBase = None): """Executes goal with specified action client and optional event that stops action execution when triggered. This is a private method. :param action_client: Action client to use. :param goal: Action goal to execute. :param stopping_event: Optional event that stops goal execution when triggered. :return: None if stopping_event is None. True if goal execution is stopped because of triggered stopping_event, False otherwise. """ if stopping_event is None: action_client.send_goal(goal) action_client.wait_for_result() return stopping_event.start_listening() action_client.send_goal(goal, active_cb=None, feedback_cb=None, done_cb=None) check_rate = rospy.Rate(100) while True: goal_state = action_client.get_state() if goal_state == SimpleGoalState.DONE: stopping_event.stop_listening() return False if stopping_event.is_triggered(): action_client.cancel_goal() return True check_rate.sleep()
class Gripper(): LEFT = 'l' RIGHT = 'r' def __init__(self, direction): assert (direction == Gripper.LEFT or direction == Gripper.RIGHT) self.direction = direction name_space = '/{0}_gripper_controller/gripper_action'.format( self.direction) self.gripper_client = SimpleActionClient(name_space, GripperCommandAction) self.gripper_client.wait_for_server() def open_gripper(self, wait=False): self.change_state(0.08, wait) def close_gripper(self, wait=False): self.change_state(0.0, wait) def change_state(self, value, wait): gripper_goal = GripperCommandGoal() gripper_goal.command.position = value gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal) if wait: self.gripper_client.wait_for_result() if not self.gripper_client.get_result().reached_goal: time.sleep(1)
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...")
class DoReset(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['reset_plan']) self.eef_pub = rospy.Publisher('/CModelRobotOutput', eef_cmd, queue_size=1, latch=True) self.move_arm_client = SimpleActionClient('/wm_arm_driver_node/execute_plan', executePlanAction) def execute(self, ud): rospy.logdebug("Entered 'RO_RESET' state.") hand_cmd = eef_cmd() hand_cmd.rACT = 1 # activate gripper hand_cmd.rGTO = 1 # request to go to position hand_cmd.rSP = 200 # set activation speed (0[slowest]-255[fastest]) hand_cmd.rFR = 0 # set force limit (0[min] - 255[max]) hand_cmd.rPR = 0 # request to open self.eef_pub.publish(hand_cmd) self.move_arm_client.wait_for_server() goal = executePlanGoal() goal.trajectory = ud.reset_plan self.move_arm_client.send_goal(goal) self.move_arm_client.wait_for_result() status = self.move_arm_client.get_state() if status == GoalStatus.SUCCEEDED: return 'succeeded' elif status == GoalStatus.PREEMPTED: return 'preempted' else: return 'aborted'
class InitMove(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'supervise']) self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) def execute(self, ud): self.move_base_client.wait_for_server() goal = MoveBaseGoal() pose = PoseStamped() pose.header.frame_id = 'map' pose.header.stamp = rospy.Time.now() pose.pose.position.x = 2.0 pose.pose.position.y = 0.0 pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 goal.target_pose = pose goal.target_pose.header.stamp = rospy.Time.now() self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result(rospy.Duration(30)) status = self.move_base_client.get_state() if status == GoalStatus.SUCCEEDED: return 'succeeded' else: return 'supervise'
class ArmController: def __init__(self, arm): self.client = SimpleActionClient( "%s_arm_controller/joint_trajectory_action" % arm, JointTrajectoryAction) #wait for the action servers to come up rospy.loginfo("[ARM] Waiting for %s controller" % arm) self.client.wait_for_server() rospy.loginfo("[ARM] Got %s controller" % arm) def start_trajectory(self, trajectory, set_time_stamp=True, wait=True): """Creates an action from the trajectory and sends it to the server""" goal = JointTrajectoryGoal() goal.trajectory = trajectory if set_time_stamp: goal.trajectory.header.stamp = rospy.Time.now() self.client.send_goal(goal) if wait: self.wait() def wait(self): self.client.wait_for_result() def is_done(self): return self.client.get_state() > SimpleGoalState.ACTIVE
def move_base(self, offset): client = SimpleActionClient( '/position_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ 'move_1_joint', 'move_2_joint', 'move_3_joint' ] point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(0.3) point.positions = offset.tolist() # point1 = JointTrajectoryPoint() # point1.time_from_start = rospy.Duration.from_sec(0.3) # print point1.time_from_start # point1.positions = [0, 0, 0] # point2 = JointTrajectoryPoint() # point2.time_from_start = rospy.Duration.from_sec(0.6) # point2.positions = [offset_x, offset_y, offset_z] goal.trajectory.points.append(point) # goal.trajectory.points.append(point2) client.send_goal(goal) client.wait_for_result()
class RecognizeGenders(smach.State): def __init__(self, **kwargs): smach.State.__init__( self, outcomes=['succeeded', 'failed'], input_keys=['image', 'number_of_faces', 'bounding_boxes']) self.timeout = kwargs.get('timeout', 10) self.say_topic = kwargs.get('say_topic', '/say') self.say_pub = rospy.Publisher(self.say_topic, String, queue_size=1) self.gender_client = SimpleActionClient( 'mdr_actions/gender_recognition_server', GenderRecognitionAction) self.gender_client.wait_for_server() def execute(self, userdata): # Recognize gender goal = GenderRecognitionGoal() goal.image = userdata.image goal.number_of_faces = userdata.number_of_faces goal.bounding_boxes = userdata.bounding_boxes self.gender_client.send_goal(goal) self.gender_client.wait_for_result() result = self.gender_client.get_result() men = result.genders.count('man') women = result.genders.count('woman') # Say x men and y women msg = String() msg.data = 'There are %i men and %i women' % (men, women) rospy.loginfo(msg.data) self.say_pub.publish(msg) return 'succeeded'
class FollowTrajectoryClient(object): """ Class to make modules (torso) follow trajectory """ def __init__(self, rospy, name, joint_names): self.rospy =rospy self.client = SimpleActionClient("%s/follow_joint_trajectory" % name, FollowJointTrajectoryAction) self.rospy.loginfo("Waiting for %s..." % name) self.client.wait_for_server() self.joint_names = joint_names def move_to(self, positions, duration = 5.0): if len(self.joint_names) != len(positions): print("Invalid trajectory position") return False trajectory = JointTrajectory() trajectory.joint_names = self.joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = positions trajectory.points[0].velocities = [0.0 for _ in positions] trajectory.points[0].accelerations = [0.0 for _ in positions] trajectory.points[0].time_from_start = self.rospy.Duration(duration) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
class Gripper: LEFT = "l" RIGHT = "r" def __init__(self, direction): assert direction == Gripper.LEFT or direction == Gripper.RIGHT self.direction = direction name_space = "/{0}_gripper_controller/gripper_action".format(self.direction) self.gripper_client = SimpleActionClient(name_space, GripperCommandAction) self.gripper_client.wait_for_server() def open_gripper(self, wait=False): self.change_state(0.08, wait) def close_gripper(self, wait=False): self.change_state(0.0, wait) def change_state(self, value, wait): gripper_goal = GripperCommandGoal() gripper_goal.command.position = value gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal) if wait: self.gripper_client.wait_for_result() if not self.gripper_client.get_result().reached_goal: time.sleep(1)
class MoveBase(smach.State): def __init__(self, destination_locations, timeout=120.0, action_server='/mdr_actions/move_base_safe_server'): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.destination_locations = list(destination_locations) self.timeout = timeout self.action_server = action_server self.client = SimpleActionClient(action_server, MoveBaseSafeAction) self.client.wait_for_server() def execute(self, userdata): goal = MoveBaseSafeGoal() for i, destination_location in enumerate(self.destination_locations): goal.destination_location = destination_location rospy.loginfo('Sending the base to %s' % destination_location) self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(self.timeout)) res = self.client.get_result() if res and res.success: rospy.loginfo('Successfully reached %s' % destination_location) else: rospy.logerr('Could not reach %s' % destination_location) return 'failed' return 'succeeded'
def on_play_2(self): change_to_controller('position') pm = SimpleActionClient('/play_motion', PlayMotionAction) pm.wait_for_server() pmg = PlayMotionGoal() pmg.motion_name = 'LBD_2X' pm.send_goal(pmg) pm.wait_for_result(rospy.Duration(0.1))
class TriggeredImageTopic(): """ This nodes provides allows to transform a trigger topic to an image topic using GrabImageAction. Whenever a signal is published on ~trigger a sensor_msg.msg.Image is published on the output topic. The interface is restricted to a rosparam for exposure with a low gain """ def __init__(self): self.camera_name = rospy.get_param('~camera_name', '') self.output_topic_name = rospy.get_param('~triggered_image_topic', 'triggered_images') if not self.camera_name: rospy.logwarn( "No camera name given! Assuming 'pylon_camera_node' as" " camera name") self.camera_name = '/pylon_camera_node' else: rospy.loginfo('Camera name is: ' + self.camera_name) self._grab_imgs_rect_ac = SimpleActionClient( '{}/grab_images_rect'.format(self.camera_name), GrabImagesAction) if self._grab_imgs_rect_ac.wait_for_server(rospy.Duration(10.0)): rospy.loginfo('Found action server at ' '{}/grab_images_raw'.format(self.camera_name)) else: rospy.logerr('Could not connect to action server at ' '{}/grab_images_raw'.format(self.camera_name)) self.pub = rospy.Publisher(self.output_topic_name, Image, queue_size=10) self.subscriber = rospy.Subscriber("~trigger", Empty, self.trigger_cb) def trigger_cb(self, msg): grab_imgs_goal = GrabImagesGoal() grab_imgs_goal.exposure_given = True grab_imgs_goal.exposure_times = [ rospy.get_param('~exposure_time', 20000.0) ] grab_imgs_goal.gain_given = True grab_imgs_goal.gain_values = [0.2] grab_imgs_goal.gain_auto = False self._grab_imgs_rect_ac.send_goal(grab_imgs_goal) self._grab_imgs_rect_ac.wait_for_result(rospy.Duration.from_sec(10.0)) grab_imgs_result = self._grab_imgs_rect_ac.get_result() rospy.loginfo("Got image") if len(grab_imgs_result.images) > 0: rospy.loginfo("publish image") self.pub.publish(grab_imgs_result.images[0]) def spin(self): # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def run_grasp(grasp, pre=False): goal = GraspGoal() goal.grasp = grasp goal.pre_grasp = pre client = SimpleActionClient('grasp', GraspAction) client.wait_for_server() client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(20.0)) if client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo("Success!") else: rospy.logerr("Fail!")
def move(lb, ub, from_location, to_location): coords = get_coords(to_location) header = Header(frame_id='/map') point = Point(**coords) quat = Quaternion(0, 0, 0, 1) pose = Pose(point, quat) move_base_client = SimpleActionClient('move_base', MoveBaseAction) move_base_client.wait_for_server() move_base_client.send_goal(MoveBaseGoal(PoseStamped(header, pose))) move_base_client.wait_for_result()
class Tiago: def __init__(self): rospy.init_node('run_motion_python') rospy.loginfo("Starting run_motion_python application...") wait_for_valid_time(10.0) self.client = SimpleActionClient('/play_motion', PlayMotionAction) rospy.loginfo("Waiting for Action Server...") self.client.wait_for_server() def act(self, action): goal = PlayMotionGoal() goal.motion_name = action goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + sys.argv[1]) self.client.send_goal(goal) rospy.loginfo("Waiting for result...") action_ok = self.client.wait_for_result(rospy.Duration(30.0)) state = self.client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state))) def reset(self): goal = PlayMotionGoal() goal.motion_name = 'home' goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + sys.argv[1]) self.client.send_goal(goal) rospy.loginfo("Waiting for result...") action_ok = self.client.wait_for_result(rospy.Duration(30.0)) state = self.client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
class safe_land(object): ''' Allows a human to teleoperate the robot through a joystick. ''' def __init__(self, name): self.robot_name = name self.state = 'IDLE' # Landing service self._land_client = SimpleActionClient( "safe_land_server", ExecuteLandAction) # Create a client to the v_search server self._land_client.wait_for_server() # Wait server to be ready self._land_goal = ExecuteLandGoal() # Message to send the goal region self.pub = rospy.Publisher("/{}/maneuvers/out".format(name), events_message, queue_size=10) # Publisher object self.msg = events_message() # Message object def execute(self): rospy.loginfo("Starting Safe Land!") self.state = 'EXE' # Start safe_land self._land_client.send_goal(self._land_goal) self._land_client.wait_for_result() state = self._land_client.get_state() # Get the state of the action print(state) if state == GoalStatus.SUCCEEDED: # safe_land successfully executed self.state = 'IDLE' # Set IDLE state self.msg.event = 'end_safe_land' self.pub.publish( self.msg ) # Send the message signaling that the safe_land is complete else: # The server aborted the motion self.state = 'ERROR' # Set ERROR state self.msg.event = 'safe_land_error' self.pub.publish(self.msg) # Send message signaling the error def reset(self): rospy.loginfo("Reseting Safe Land!") self.state = 'IDLE' def erro(self): rospy.loginfo("Safe Land Erro!") self.state = 'ERROR' # Set ERROR state self._land_client.cancel_goal() # Cancel the motion of the robot
class MoveBaseClient(object): """ Class to move the fetch base to desired position """ def __init__(self, rospy): # Constants for the class ROSTOPIC_NAME = "move_base" # Initialization self.rospy = rospy self.client = SimpleActionClient(ROSTOPIC_NAME, MoveBaseAction) self.rospy.loginfo("Waiting for /move_base ....") self.client.wait_for_server() self.rospy.loginfo("Connected to /move_base") def goto2d(self, x, y, theta = 0.0, frame = "map"): """ Structure: target_pose: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0 """ move_goal = MoveBaseGoal() move_goal.target_pose.pose.position.x = x move_goal.target_pose.pose.position.y = y move_goal.target_pose.pose.orientation.z = sin(theta/2.0) move_goal.target_pose.pose.orientation.w = cos(theta/2.0) move_goal.target_pose.header.frame_id = frame move_goal.target_pose.header.stamp = self.rospy.Time.now() self.client.send_goal(move_goal) self.client.wait_for_result() #self.client.send_goal_and_wait(move_goal,execute_timeout=rospy.Duration(10,0), preempt_timeout=rospy.Duration(20,0)) def goto3d(self): """ Function not implemented """ pass
class fibonacci_client: def __init__(self, name): self.action_client = SimpleActionClient('fibonacci_server', FibonacciAction) self.action_client.wait_for_server() goal = FibonacciGoal(order=20) self.action_client.send_goal(goal) self.action_client.wait_for_result() result = self.action_client.get_result() rospy.loginfo('[Fibonacci Client] Result: {}'.format(', '.join( [str(n) for n in result.sequence])))
class perceive_cavity(smach.State): def __init__(self): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("perceive_cavity_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return "success" else: return "failed"
def applyEndPosition(self): rospy.loginfo("Initializing dmp_execution test.") diction = [] for i in range(0, len(self.plan_resp.plan.points)): diction.append({ 'positions': self.plan_resp.plan.points[i].positions, 'time_from_start': self.plan_resp.plan.times[i] }) name = 'movement' d = { 'play_motion': { 'controllers': self.controllers, 'motions': { name: { 'joints': self.joint_names, 'points': diction } } } } # Create params rospy.set_param('/play_motion/motions/movement/points', diction) rospy.set_param('/play_motion/motions/movement/joints', self.joint_names) #rospy.loginfo("Parameters created") client = SimpleActionClient("play_motion", PlayMotionAction) client.wait_for_server() rospy.loginfo("...connected.") #rospy.wait_for_message("joint_states", JointState) #rospy.sleep(3.0) rospy.loginfo("Initial position...") goal = PlayMotionGoal() goal.motion_name = 'movement' goal.skip_planning = True #raw_input('Press Enter to execute action\n') client.send_goal(goal) client.wait_for_result(rospy.Duration(15.0)) rospy.loginfo("Movement reached.") rospy.loginfo("Action done")
class GripperController: def __init__(self, hand): arm = hand[0] self.effort = -1.0 self.server = SimpleActionServer(ACTION_NAME % arm, GripperSequenceAction, self.execute, False) self.server.start() self.sub_client = SimpleActionClient( "%s_gripper_controller/gripper_action" % arm, Pr2GripperCommandAction) #wait for the action servers to come up rospy.loginfo("[GRIPPER] Waiting for %s controllers" % arm) self.sub_client.wait_for_server() rospy.loginfo("[GRIPPER] Got %s controllers" % arm) self.client = SimpleActionClient(ACTION_NAME % arm, GripperSequenceAction) rospy.loginfo("[GRIPPER] Waiting for self client") self.client.wait_for_server() rospy.loginfo("[GRIPPER] Got self client") def send_goal(self, goal): self.client.send_goal(goal) def execute(self, goal): rate = rospy.Rate(10) while rospy.Time.now() < goal.header.stamp: rate.sleep() for position, time in zip(goal.positions, goal.times): self.change_position(position, False) rospy.sleep(time) self.server.set_succeeded(GripperSequenceResult()) self.change_position(position, False, 0.0) def change_position(self, position, should_wait=True, effort=None): if effort is None: effort = self.effort gg = Pr2GripperCommandGoal() gg.command.position = position gg.command.max_effort = effort self.sub_client.send_goal(gg) if should_wait: self.sub_client.wait_for_result()
class execute_plan(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['success', 'failure', 'preempted'], input_keys=['already_once_executed', 'plan'], output_keys=['already_once_executed']) self.planner_executor_client = SimpleActionClient( '/task_planning/execute_plan', ExecutePlanAction) self.planner_executor_client.wait_for_server() rospy.sleep(0.2) def execute(self, userdata): userdata.already_once_executed = True success = self.execute_plan(userdata.plan) if not success: return 'failure' return 'success' def execute_plan(self, plan): goal = ExecutePlanGoal() goal.plan = plan self.planner_executor_client.send_goal(goal) finished = self.planner_executor_client.wait_for_result() if not finished: return (False) res = self.planner_executor_client.get_result() return res.success
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 look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class perceive_location(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('perceive_location_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return 'success' else: return 'failed'
class Enter(ScenarioStateBase): def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'enter_door', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying'], output_keys=['enter_door_feedback']) self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.enter_door_server = kwargs.get('enter_action_server', 'enter_door_server') self.enter_action_client = SimpleActionClient(self.enter_door_server, EnterDoorAction) self.enter_action_client.wait_for_server(rospy.Duration(10.)) def execute(self, userdata): goal = EnterDoorGoal() rospy.loginfo('[ENTER_DOOR] Calling door entering action') self.say('Entering door') self.enter_action_client.send_goal(goal) duration = rospy.Duration.from_sec(self.timeout) success = self.enter_action_client.wait_for_result(duration) if success: rospy.loginfo('Entered successfully') return 'succeeded' else: rospy.logerr('Entering failed') self.say('Could not enter door') if self.retry_count == self.number_of_retries: self.say('Aborting operation') return 'failed_after_retrying' return 'failed'
class HeadNode(): def __init__(self): name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() def moveHead(self, theta, phi): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(math.cos(theta), math.sin(theta), phi) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration(10.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class Torso: def __init__(self): name_space = '/torso_controller/position_joint_action' self.torso_client = SimpleActionClient(name_space, SingleJointPositionAction) self.torso_client.wait_for_server() def move(self, position): assert(position >= 0.0 and position <= 0.2) self.position = position up = SingleJointPositionGoal() up.position = self.position up.min_duration = rospy.Duration(2.0) up.max_velocity = 1.0 self.torso_client.send_goal(up) self.torso_client.wait_for_result()
def testsimple(self): client = SimpleActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(10.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual("The ref server has succeeded", client.get_goal_status_text()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) self.assertEqual("The ref server has aborted", client.get_goal_status_text())
def move_head(): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = self.get_frame() head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = self.get_target() head_goal.max_velocity = 10.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(2.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') self.gui.show_text_in_rviz("Head!")
def tilt_head(self, down=True): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = self.get_frame() head_goal.min_duration = rospy.Duration(0.3) if down: head_goal.target.point = Point(1, 0, Head.speed * -0.1) else: head_goal.target.point = Point(1, 0, Head.speed * 0.1) head_goal.max_velocity = 10.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(2.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class Move_Head: """ a class to fins and pick up a object """ def __init__(self, text): #self.task = text#whole message from greeting self.head_client = SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.head_client.wait_for_server() ar = text.split(':') cmd = ar[0] direction = ar[5] if direction =='left': self.move_head(1.1, 0.0) if direction =='right': self.move_head(-1.1, 0.0) if direction =='front': self.move_head(0.0, 0.0) def move_head(self, pan , tilt): # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_mod_joint'] # Create a single-point head trajectory with the head_goal as the end-point head_trajectory = JointTrajectory() head_trajectory.joint_names = head_joints head_trajectory.points.append(JointTrajectoryPoint()) head_trajectory.points[0].positions = pan , tilt head_trajectory.points[0].velocities = [0.0 for i in head_joints] head_trajectory.points[0].accelerations = [0.0 for i in head_joints] head_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = head_trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal self.head_client.send_goal(head_goal) # Wait for up to 5 seconds for the motion to complete self.head_client.wait_for_result(rospy.Duration(2.0))
def main(): rospy.init_node("send_reference") ref_args = sys.argv[1:] if not ref_args: rospy.logwarn("Usage: send_reference JOINT_NAME_1 POSITION_1 [ JOINT_NAME_2 POSITION 2 ... ]") return 1 ac_joint_traj = SimpleActionClient("/amigo/body/joint_trajectory_action", FollowJointTrajectoryAction) while not ac_joint_traj.wait_for_server(timeout=rospy.Duration(1.0)) and not rospy.is_shutdown(): rospy.logwarn("Waiting for server...") if rospy.is_shutdown(): rospy.logwarn("No reference sent") return 1 rospy.loginfo("Connected to server") # - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Even items in ref_args are joint_names joint_names = ref_args[0::2] # Odd items in ref_args are set points ref_positions = [float(i) for i in ref_args[1::2]] # - - - - - - - - - - - - - - - - - - - - - - - - - - - - points = [JointTrajectoryPoint(positions=ref_positions, time_from_start=rospy.Duration(0))] joint_trajectory = JointTrajectory(joint_names=joint_names, points=points) goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=rospy.Duration(1000)) ac_joint_traj.send_goal(goal) ac_joint_traj.wait_for_result(rospy.Duration(1000)) res = ac_joint_traj.get_result() if res.error_string: rospy.logerr(res.error_string)
def move_gripper(): name_space = "/{0}_gripper_controller/gripper_action".format(self.direction) gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_client.wait_for_server() gripper_goal = GripperCommandGoal() if self.state: # The gripper is open. Close it and update its state. gripper_goal.command.position = 0.0 self.state = Gripper.CLOSED else: # The gripper is closed. Open it and update its state. gripper_goal.command.position = 0.08 self.state = Gripper.OPEN gripper_goal.command.max_effort = 30.0 gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(10.0)) self.gui.show_text_in_rviz("Gripper")
def main(): rospy.init_node("simple_navigation_goals") move_base_client = SimpleActionClient('move_base', MoveBaseAction) rospy.loginfo('Connecting to server') move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'komodo_1/base_link' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = -1.0 goal.target_pose.pose.orientation.w = 1.0 rospy.loginfo('Sending goal') move_base_client.send_goal(goal) move_base_client.wait_for_result() if move_base_client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Hooray, the base moved 1 meter forward') else: rospy.loginfo('The base failed to move forward 1 meter for some reason')
class TrajectoryArmController: def __init__( self, controller_name ): topic = ''.join( ( controller_name, '/joint_trajectory_action' ) ) self.client = SimpleActionClient( topic, JointTrajectoryAction ) rospy.loginfo( 'waiting for action client: %s'%topic ) self.client.wait_for_server() def move( self, trajectory ): # push trajectory goal to ActionServer goal = JointTrajectoryGoal() goal.trajectory = trajectory goal.trajectory.header.stamp = rospy.Time.now() self.client.send_goal( goal ) self.client.wait_for_result() if self.client.get_state == GoalStatus.SUCCEEDED: rospy.logerr( 'failed to move arm to goal:\n %s'%goal ) return False else: return True
def test_one(self): client = SimpleActionClient('reference_simple_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(2.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(3) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") #The simple server can't reject goals self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(9) saved_feedback={}; def on_feedback(fb): rospy.loginfo("Got feedback") saved_feedback[0]=fb client.send_goal(goal,feedback_cb=on_feedback) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual(saved_feedback[0].feedback,9)
class ObjectSoundCollector: def __init__(self): self.gripper_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction) self.gripper_client.wait_for_server() def close_gripper(self, torque_limit, dynamic_torque_control): goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.CLOSE_GRIPPER goal.torque_limit = torque_limit goal.dynamic_torque_control = dynamic_torque_control goal.pressure_upper = 1500.0 goal.pressure_lower = 1300.0 self.gripper_client.send_goal(goal) self.gripper_client.wait_for_result() def open_gripper(self): goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.OPEN_GRIPPER goal.torque_limit = 0.4 self.gripper_client.send_goal(goal) self.gripper_client.wait_for_result()
def head_action(self, x, y, z, wait = False): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(x, y, z) head_client.send_goal(head_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = head_client.wait_for_result(rospy.Duration(5)) # Check for success or failure if not finished_within_time: head_client.cancel_goal() rospy.loginfo("Timed out achieving head movement goal") else: state = head_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Head movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
class LocalSearch(): VISUAL_FIELD_SIZE = 40 MIN_HEAD_ANGLE = -140 MAX_HEAD_ANGLE = 140 _feedback = LocalSearchFeedback() _result = LocalSearchResult() def __init__(self): self._action_name = 'local_search' self.found_marker = False self.tracking_started = False #initialize head mover name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() rospy.loginfo('%s: Action client for PointHeadAction running' % self._action_name) #initialize tracker mark rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_tracker) rospy.loginfo('%s: subscribed to AlvarMarkers' % self._action_name) #initialize this client self._as = SimpleActionServer(self._action_name, LocalSearchAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def marker_tracker(self, marker): if self.tracking_started: self.found_marker = True rospy.loginfo('%s: marker found' % self._action_name) def run(self, goal): success = False self.tracking_started = True self.found_marker = False rospy.loginfo('%s: Executing search for AR marker' % self._action_name) # define a set of ranges to search search_ranges = [ # first search in front of the robot (0, self.VISUAL_FIELD_SIZE), (self.VISUAL_FIELD_SIZE, -self.VISUAL_FIELD_SIZE), # then search all directions (-self.VISUAL_FIELD_SIZE, self.MAX_HEAD_ANGLE), (self.MAX_HEAD_ANGLE, self.MIN_HEAD_ANGLE), (self.MIN_HEAD_ANGLE, 0) ] range_index = 0 #success = self.search_range(*(search_ranges[range_index])) while (not success) and range_index < len(search_ranges) - 1: if self._as.is_preempt_requested(): rospy.loginfo('%s: Premepted' % self._action_name) self._as.set_preempted() break range_index = range_index + 1 success = self.search_range(*(search_ranges[range_index])) if success: rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded() else: self._as.set_aborted() self.tracking_started = False def search_range(self, start_angle, end_angle): rospy.loginfo("{}: searching range {} {}".format(self._action_name, start_angle, end_angle)) angle_tick = self.VISUAL_FIELD_SIZE if (start_angle < end_angle) else -self.VISUAL_FIELD_SIZE for cur_angle in xrange(start_angle, end_angle, angle_tick): if self._as.is_preempt_requested(): return False head_goal = self.lookat_goal(cur_angle) rospy.loginfo('%s: Head move goal for %s: %s produced' % (self._action_name, str(cur_angle), str(head_goal))) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration.from_sec(5.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head could not move to specified location') break # pause at each tick rospy.sleep(0.3) if (self.found_marker): # found a marker! return True # no marker found return False def lookat_goal(self, angle): head_goal = PointHeadGoal() head_goal.target.header.frame_id = '/torso_lift_link' head_goal.max_velocity = 0.8 angle_in_radians = math.radians(angle) x = math.cos(angle_in_radians) * 5 y = math.sin(angle_in_radians) * 5 z = -0.5 head_goal.target.point = Point(x, y, z) return head_goal
class Kill(): REFRESH_RATE = 20 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 marker_callback(self, marker): if (self.pose is not None): self.pose = marker.pose def run(self, goal): rospy.loginfo('Begin kill') self.pose = goal.pose #pose = self._tf.transformPose('/base_link', goal.pose) self._sound_client.say('Halt!') # turn to face the marker before opening arms (code repeated later) r = rospy.Rate(self.REFRESH_RATE) while(True): pose = self.pose if abs(pose.pose.position.y) > .1: num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / ( 3 * abs(pose.pose.position.y))) for i in range(num_move_y): if pose != self.pose: break self._base_publisher.publish(twist_msg) r.sleep() pose.pose.position.y = 0 else: break # open arms traj_goal_r = JointTrajectoryGoal() traj_goal_r.trajectory.joint_names = self.r_joint_names traj_goal_l = JointTrajectoryGoal() traj_goal_l.trajectory.joint_names = self.l_joint_names traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2))) self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3)) traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2))) self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3)) traj_goal_r.trajectory.points[0].positions = self.r2 self.r_traj_action_client.send_goal(traj_goal_r) traj_goal_l.trajectory.points[0].positions = self.l2 self.l_traj_action_client.send_goal(traj_goal_l) self._sound_client.say('You have the right to remain silent.') # Keep a local copy because it will update #pose = self.pose #num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) #twist_msg = Twist() #twist_msg.linear = Vector3(.1, 0.0, 0.0) #twist_msg.angular = Vector3(0.0, 0.0, 0.0) #for i in range(num_move_x): # self._base_publisher.publish(twist_msg) # r.sleep() # track the marker as much as we can while True: pose = self.pose # too far away if abs(pose.pose.position.x > 1.5): rospy.loginfo('Target out of range: ' + str(pose.pose.position.x)) self._as.set_aborted() return; # too far to the side -> rotate elif abs(pose.pose.position.y) > .1: num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y))) for i in range(num_move_y): if pose != self.pose: break self._base_publisher.publish(twist_msg) r.sleep() pose.pose.position.y = 0 #twist_msg = Twist() #twist_msg.linear = Vector3(0.0, 0.0, 0.0) #twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y))) #self._base_publisher.publish(twist_msg) # now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over) elif pose.pose.position.x > .5: num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) twist_msg = Twist() twist_msg.linear = Vector3(.1, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for i in range(num_move_x): if pose != self.pose: break self._base_publisher.publish(twist_msg) r.sleep() pose.pose.position.x = 0 #twist_msg = Twist() #twist_msg.linear = Vector3(.1, 0.0, 0.0) #twist_msg.angular = Vector3(0.0, 0.0, 0.0) #self._base_publisher.publish(twist_msg) # susupect is within hugging range!!! else: break r.sleep() # after exiting the loop, we should be ready to capture -> send close arms goal self._sound_client.say("Anything you say do can and will be used against you in a court of law.") self.l_traj_action_client.wait_for_result(rospy.Duration(3)) self.r_traj_action_client.wait_for_result(rospy.Duration(3)) traj_goal_r.trajectory.points[0].positions = self.r3 self.r_traj_action_client.send_goal(traj_goal_r) traj_goal_l.trajectory.points[0].positions = self.l3 self.l_traj_action_client.send_goal(traj_goal_l) self.l_traj_action_client.wait_for_result(rospy.Duration(3)) self.r_traj_action_client.wait_for_result(rospy.Duration(3)) rospy.loginfo('End kill') rospy.sleep(20) self._as.set_succeeded()
class Arm: ''' Interfacing with one arm for controlling mode and action execution''' _is_autorelease_on = False def __init__(self, arm_index): self.arm_index = arm_index self.arm_mode = ArmMode.HOLD self.gripper_state = None self.gripper_joint_name = self._side_prefix() + '_gripper_joint' self.ee_name = self._side_prefix() + '_wrist_roll_link' self.joint_names = [self._side_prefix() + '_shoulder_pan_joint', self._side_prefix() + '_shoulder_lift_joint', self._side_prefix() + '_upper_arm_roll_joint', self._side_prefix() + '_elbow_flex_joint', self._side_prefix() + '_forearm_roll_joint', self._side_prefix() + '_wrist_flex_joint', self._side_prefix() + '_wrist_roll_joint'] self.all_joint_names = [] self.all_joint_poses = [] self.last_ee_pose = None self.movement_buffer_size = 40 self.last_unstable_time = rospy.Time.now() self.arm_movement = [] rospy.loginfo('Initializing ' + self._side() + ' arm.') switch_controller = 'pr2_controller_manager/switch_controller' rospy.wait_for_service(switch_controller) self.switch_service = rospy.ServiceProxy(switch_controller, SwitchController) rospy.loginfo('Got response form the switch controller for ' + self._side() + ' arm.') # Create a trajectory action client traj_controller_name = (self._side_prefix() + '_arm_controller/joint_trajectory_action') self.traj_action_client = SimpleActionClient( traj_controller_name, JointTrajectoryAction) self.traj_action_client.wait_for_server() rospy.loginfo('Got response form trajectory action server for ' + self._side() + ' arm.') # Set up Inversse Kinematics self.ik_srv = None self.ik_request = None self.ik_joints = None self.ik_limits = None self._setup_ik() gripper_name = (self._side_prefix() + '_gripper_controller/gripper_action') self.gripper_client = SimpleActionClient(gripper_name, Pr2GripperCommandAction) self.gripper_client.wait_for_server() rospy.loginfo('Got response form gripper server for ' + self._side() + ' arm.') filter_srv_name = '/trajectory_filter/filter_trajectory' rospy.wait_for_service(filter_srv_name) self.filter_service = rospy.ServiceProxy(filter_srv_name, FilterJointTrajectory) rospy.loginfo('Filtering service has responded for ' + self._side() + ' arm.') self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) def _setup_ik(self): '''Sets up services for inverse kinematics''' ik_info_srv_name = ('pr2_' + self._side() + '_arm_kinematics_simple/get_ik_solver_info') ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik' rospy.wait_for_service(ik_info_srv_name) ik_info_srv = rospy.ServiceProxy(ik_info_srv_name, GetKinematicSolverInfo) solver_info = ik_info_srv() rospy.loginfo('IK info service has responded for ' + self._side() + ' arm.') rospy.wait_for_service(ik_srv_name) self.ik_srv = rospy.ServiceProxy(ik_srv_name, GetPositionIK, persistent=True) rospy.loginfo('IK service has responded for ' + self._side() + ' arm.') # Set up common parts of an IK request self.ik_request = GetPositionIKRequest() self.ik_request.timeout = rospy.Duration(4.0) self.ik_joints = solver_info.kinematic_solver_info.joint_names self.ik_limits = solver_info.kinematic_solver_info.limits ik_links = solver_info.kinematic_solver_info.link_names request = self.ik_request.ik_request request.ik_link_name = ik_links[0] request.pose_stamped.header.frame_id = 'base_link' request.ik_seed_state.joint_state.name = self.ik_joints request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints) def _side(self): '''Returns the word right or left depending on arm side''' if (self.arm_index == Side.RIGHT): return 'right' elif (self.arm_index == Side.LEFT): return 'left' def _side_prefix(self): ''' Returns the letter r or l depending on arm side''' side = self._side() return side[0] def get_ee_state(self, ref_frame='base_link'): ''' Returns end effector pose for the arm''' try: time = World.tf_listener.getLatestCommonTime(ref_frame, self.ee_name) (position, orientation) = World.tf_listener.lookupTransform( ref_frame, self.ee_name, time) ee_pose = Pose() ee_pose.position = Point(position[0], position[1], position[2]) ee_pose.orientation = Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) return ee_pose except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('Something wrong with transform request.') return None def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.lock.release() def get_joint_state(self, joint_names=None): '''Returns position for the requested or all arm joints''' if joint_names == None: joint_names = self.joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received!\n") return [] positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return positions def _solve_ik(self, ee_pose, seed=None): '''Gets the IK solution for end effector pose''' self.ik_request.ik_request.pose_stamped.pose = ee_pose if seed == None: # If no see is specified for IK search, start search at midpoint seed = [] for i in range(0, len(self.ik_joints)): seed.append((self.ik_limits[i].min_position + self.ik_limits[i].max_position) / 2.0) self.ik_request.ik_request.ik_seed_state.joint_state.position = seed try: rospy.loginfo('Sending IK request.') response = self.ik_srv(self.ik_request) if(response.error_code.val == response.error_code.SUCCESS): return response.solution.joint_state.position else: return None except rospy.ServiceException: rospy.logerr('Exception while getting the IK solution.') return None def set_mode(self, mode): '''Releases or holds the arm by turning the controller on/off''' controller_name = self._side_prefix() + '_arm_controller' if mode == ArmMode.RELEASE: start_controllers = [] stop_controllers = [controller_name] rospy.loginfo('Switching ' + str(self._side()) + ' arm to the kinesthetic mode') elif mode == ArmMode.HOLD: start_controllers = [controller_name] stop_controllers = [] rospy.loginfo('Switching ' + str(self._side()) + ' to the Joint-control mode') else: rospy.logwarn('Unknown mode ' + str(mode) + '. Keeping the current mode.') return try: self.switch_service(start_controllers, stop_controllers, 1) self.arm_mode = mode except rospy.ServiceException: rospy.logerr("Service did not process request") def get_mode(self): '''Returns the current arm mode (released/holding)''' return self.arm_mode def _send_gripper_command(self, pos=0.08, eff=30.0, wait=False): '''Sets the position of the gripper''' command = Pr2GripperCommandGoal() command.command.position = pos command.command.max_effort = eff self.gripper_client.send_goal(command) if wait: self.gripper_client.wait_for_result(rospy.Duration(10.0)) def is_gripper_moving(self): ''' Whether or not the gripper is in the process of opening/closing''' return (self.gripper_client.get_state() == GoalStatus.ACTIVE or self.gripper_client.get_state() == GoalStatus.PENDING) def is_gripper_at_goal(self): ''' Whether or not the gripper has reached its goal''' return (self.gripper_client.get_state() == GoalStatus.SUCCEEDED) def get_gripper_state(self): '''Returns current gripper state''' return self.gripper_state def check_gripper_state(self, joint_name=None): '''Checks gripper state at the hardware level''' if (joint_name == None): joint_name = self.gripper_joint_name gripper_pos = self.get_joint_state([joint_name]) if gripper_pos != []: if gripper_pos[0] > 0.078: self.gripper_state = GripperState.OPEN else: self.gripper_state = GripperState.CLOSED else: rospy.logwarn('Could not update the gripper state.') def open_gripper(self, pos=0.08, eff=30.0, wait=False): '''Opens gripper''' self._send_gripper_command(pos, eff, wait) self.gripper_state = GripperState.OPEN def close_gripper(self, pos=0.0, eff=30.0, wait=False): '''Closes gripper''' self._send_gripper_command(pos, eff, wait) self.gripper_state = GripperState.CLOSED def set_gripper(self, gripper_state): '''Sets gripper to the desired state''' if (gripper_state == GripperState.CLOSED): self.close_gripper() elif (gripper_state == GripperState.OPEN): self.open_gripper() def exectute_joint_traj(self, joint_trajectory, timing): '''Moves the arm through the joint sequence''' # First, do filtering on the trajectory to fix the velocities trajectory = JointTrajectory() # Initialize the server # When to start the trajectory: 0.1 seconds from now trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) trajectory.joint_names = self.joint_names ## Add all frames of the trajectory as way points for i in range(len(timing)): positions = joint_trajectory[i].joint_pose velocities = [0] * len(positions) # Add frames to the trajectory trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=timing[i])) output = self.filter_service(trajectory=trajectory, allowed_time=rospy.Duration.from_sec(20)) rospy.loginfo('Trajectory for arm ' + str(self.arm_index) + ' has been filtered.') traj_goal = JointTrajectoryGoal() # TO-DO: check output.error_code traj_goal.trajectory = output.trajectory traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.joint_names = self.joint_names # Sends the goal to the trajectory server self.traj_action_client.send_goal(traj_goal) def move_to_joints(self, joints, time_to_joint): '''Moves the arm to the desired joints''' # Setup the goal traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.joint_names = self.joint_names velocities = [0] * len(joints) traj_goal.trajectory.points.append(JointTrajectoryPoint( positions=joints, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) # Client sends the goal to the Server self.traj_action_client.send_goal(traj_goal) def is_executing(self): '''Whether or not there is an ongoing action execution on the arm''' return (self.traj_action_client.get_state() == GoalStatus.ACTIVE or self.traj_action_client.get_state() == GoalStatus.PENDING) def is_successful(self): '''Whetehr the execution succeeded''' return (self.traj_action_client.get_state() == GoalStatus.SUCCEEDED) def get_ik_for_ee(self, ee_pose, seed): ''' Finds the IK solution for given end effector pose''' joints = self._solve_ik(ee_pose, seed) ## If our seed did not work, try once again with the default seed if joints == None: rospy.logwarn('Could not find IK solution with preferred seed,' + 'will try default seed.') joints = self._solve_ik(ee_pose) if joints == None: rospy.logwarn('IK out of bounds, will use the seed directly.') else: rollover = array((array(joints) - array(seed)) / pi, int) joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi return joints @staticmethod def get_distance_bw_poses(pose0, pose1): '''Returns the dissimilarity between two end-effector poses''' w_pos = 1.0 w_rot = 0.2 pos0 = array((pose0.position.x, pose0.position.y, pose0.position.z)) pos1 = array((pose1.position.x, pose1.position.y, pose1.position.z)) rot0 = array((pose0.orientation.x, pose0.orientation.y, pose0.orientation.z, pose0.orientation.w)) rot1 = array((pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w)) pos_dist = w_pos * norm(pos0 - pos1) rot_dist = w_rot * (1 - dot(rot0, rot1)) if (pos_dist > rot_dist): dist = pos_dist else: dist = rot_dist return dist def reset_movement_history(self): ''' Clears the saved history of arm movements''' self.last_unstable_time = rospy.Time.now() self.arm_movement = [] def get_movement(self): '''Returns cumulative movement in recent history''' return sum(self.arm_movement) def _record_arm_movement(self, reading): '''Records the sensed arm movement''' self.arm_movement = [reading] + self.arm_movement if (len(self.arm_movement) > self.movement_buffer_size): self.arm_movement = self.arm_movement[0:self.movement_buffer_size] def _is_arm_moved_while_holding(self): '''Checks if user is trying to move the arm while it is stiff''' threshold = 0.02 if (self.get_mode() == ArmMode.HOLD and (len(self.arm_movement) == self.movement_buffer_size) and (self.get_movement() > threshold)): return True return False def _is_arm_stable_while_released(self): '''Checks if the arm has been stable while being released''' movement_threshold = 0.02 time_threshold = rospy.Duration(5.0) is_arm_stable = (self.get_movement() < movement_threshold) if (not is_arm_stable or self.get_mode() == ArmMode.HOLD): self.last_unstable_time = rospy.Time.now() return False else: if (rospy.Time.now() - self.last_unstable_time) > time_threshold: return True else: return False def update(self, is_executing): ''' Periodical update for one arm''' ee_pose = self.get_ee_state() if (ee_pose != None and self.last_ee_pose != None): self._record_arm_movement(Arm.get_distance_bw_poses(ee_pose, self.last_ee_pose)) self.last_ee_pose = ee_pose if (not is_executing and Arm._is_autorelease_on): if (self._is_arm_moved_while_holding()): rospy.loginfo('Automatically releasing arm.') self.set_mode(ArmMode.RELEASE) if (self._is_arm_stable_while_released()): rospy.loginfo('Automatically holding arm.') self.set_mode(ArmMode.HOLD)
class bin_bot_base(): def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z): #global self.speed = Twist() # variable to store velocity commands self.tracked_blob = Twist() # variable to store pose of tracked blob self.state = FSM_STATES.FSM_LOCALIZE # set initial state self.obj_pose = MoveBaseGoal() # variable self.goal = MoveBaseGoal() self.blobs_3d = Blobs3d() self.blobs_detected = False self.miss_counter = 0 self.zStable = False # flag to signify desired z satisfied self.angStable = False # flag to signify desired angle satisfied self.arm_state = String #params self.desired_z = desired_z #0.001 #0.8 self.KpLin = KpLin #1000.0 self.KdLin = KdLin self.KpRot = KpRot #0.1 self.KdRot = KdRot self.tol_z = tol_z #0.00005 #0.05 self.tol_ang= tol_ang #0.5 self.maxLinSpeed = maxLinSpeed #0.2 self.maxRotSpeed = maxRotSpeed self.max_z = max_z #3.0 self.min_z= min_z #0.0005 #0.2 # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) self._move_base.wait_for_server() #subscribtions rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback) rospy.Subscriber('/bin_bot_base/goal', PoseStamped, self.goal_callback) rospy.Subscriber('/arm_node/state', String, self.arm_state_callback) # publications self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity self.state_pub = rospy.Publisher('/bin_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity ################# # goal_callback : Run everytime '\arm_bot_base\goal' topic receives message. # # Send a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ################# # blob_callback : Run everytime \blobs_3d receives message. ################# def blob_callback(self, blobs_3d): self.blobs_3d = blobs_3d self.blob_area = 0 if(len(self.blobs_3d.blobs)): self.blobs_detected = True self.miss_counter = 0 for blob in blobs_3d.blobs: if (blob.area>self.blob_area): self.blob_area = blob.area self.tracked_blob.linear = self.getPose(blob) else: if(self.miss_counter>=5): # Allow for 5 callbacks before signaling non-detection self.blobs_detected = False self.miss_counter += 1 ###################### # arm_state_callback : Run everytime the arm updates it's state. ###################### def arm_state_callback(self, arm_state): self.arm_state = arm_state ################# # speed_callback : Pubishes velocity and handles state transitions ################# def speed_callback(self): self.moveDuration = rospy.Duration(3) # duration of forward movement # Print state print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_LOCALIZE STATE # =============> if self.state == FSM_STATES.FSM_LOCALIZE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transition # if-else statement to ensure error-less view of goal status if(self.blobs_detected): # if a blob is seen rospy.loginfo("Arm Bot detected! Approaching robot...") self.state = FSM_STATES.FSM_OPTIMIZE elif (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) self.state = FSM_STATES.FSM_LOCALIZE else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) self.state = FSM_STATES.FSM_NAVIGATE # FSM_NAVIGATE STATE # =============> elif self.state == FSM_STATES.FSM_NAVIGATE: # No action required, simply allow path planner to navigate self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transitions if(self.blobs_detected): # and (self.tracked_blob.linear.z < 3.0): # if a blob is seen (and closer than 3 meters) self._move_base.cancel_all_goals() # cancell all nav goals rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) rospy.loginfo("Arm Bot detected! Aborting goal and approaching robot...") self.state = FSM_STATES.FSM_OPTIMIZE else: if(self._move_base.get_state() != 3.0): rospy.loginfo("Navigating to Arm Bot at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z)) else: rospy.loginfo("Waiting for goal...") rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) self.state = FSM_STATES.FSM_NAVIGATE # FSM_OPTIMIZE STATE (PID/PD controller) # =================> elif self.state == FSM_STATES.FSM_OPTIMIZE : rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z)) #self.LinDerivator = 0.0 self.RotDerivator = 0.0 # if statement to check Arm Bot is with range (TO BE DELETED/AMENDED!) if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 rospy.loginfo("Detected object is either too close or too far away.") else: # Set linear speed to 0 self.speed.linear.x = 0.0 # Compute PD angular error self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y); self.pRotError = self.KpRot*self.angError self.dRotError = self.KdRot*(self.angError-self.RotDerivator) self.RotDerivator = self.angError self.anglePID = self.pRotError + self.dRotError; # Cap maximum rotation speed if(math.fabs(self.anglePID)>self.maxRotSpeed): self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID; # Set tolerance in angle to signify stable orientation if(math.fabs(self.angError) < self.tol_ang): self.speed.angular.z = 0.0; RotDerivator = 0.0 self.angStable = True else: self.speed.angular.z = self.anglePID; self.angStable = False # Debug messages rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError)) # Next state transitions if (self.angStable): # if stable position and orientation have been achieved self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state self.moveStartTime = rospy.Time.now() elif(self.blobs_detected): # else if (while) blobs are being detected self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state else: self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state # FSM_APPROACH_OBJ STATE # =============> elif self.state == FSM_STATES.FSM_APPROACH_OBJ: self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' self.obj_pose.target_pose.header.stamp = rospy.Time.now() self.obj_pose.target_pose.pose.orientation.w = 1.0 # if-else statements to perform corrections in target pose # if distance is higher than 2.20m if ( self.tracked_blob.linear.z > 0.0022): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 2m) self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing approach to 2m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 2m and more than 1.20m elif (self.tracked_blob.linear.z > 0.0012): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m) self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 1m else: self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.moveStartTime = rospy.Time.now() self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN # FSM_SPIN STATE # =============> elif (self.state == FSM_STATES.FSM_SPIN): # Spin if (rospy.Time.now() < self.moveStartTime + self.moveDuration): self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 self.state = FSM_STATES.FSM_SPIN else: self.state = FSM_STATES.FSM_WAIT_FOR_DROP # FSM_WAIT_FOR_DROP STATE # =============> elif (self.state == FSM_STATES.FSM_WAIT_FOR_DROP): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transitions if (self.arm_state.data == 'OBJ_DROPPED'): # if arm signifies that object has been dropped self.state = FSM_STATES.FSM_MOVE_TO_BASE # change to MOVE_TO_BASE state else: # if object not dropped self.state = FSM_STATES.FSM_WAIT_FOR_DROP # loop back # FSM_MOVE_TO_BASE STATE # =============> elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE): # **TEST** wait for coordinates of base to be passed and then move to base if (self._move_base.simple_state == 2): rospy.loginfo("Waiting for base coordinates...") self.state = FSM_STATES.FSM_MOVE_TO_BASE else: # if a goal is detected rospy.loginfo("Moving back to base") rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_NAVIGATE # Print general info self.printINFO() # Publish /arm_bot_base/state self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) # Only publish velocity when no goal is being pursued or expected if (self.state != FSM_STATES.FSM_NAVIGATE and self.state != FSM_STATES.FSM_MOVE_TO_BASE and self.state != FSM_STATES.FSM_APPROACH_OBJ): self.speed_pub.publish(self.speed) ############ # getPose(): Returns pose of received blob relative to the camera. ############ def getPose(self, blob): self.pose = Vector3() self.pose.x = blob.center.x self.pose.y = blob.center.y self.pose.z = blob.center.z return self.pose def printINFO(self): # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) if(self.blobs_detected): rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("miss_counter = %f" %(self.miss_counter)) else: rospy.loginfo("No blobs detected") rospy.loginfo("miss_counter = %f" %(self.miss_counter)) def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.speed_callback() rate.sleep()
class HERBRobot(Robot): def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim, left_hand_sim, right_hand_sim, left_ft_sim, head_sim, talker_sim, segway_sim, perception_sim): from prpy.util import FindCatkinResource Robot.__init__(self, robot_name='herb') # Controller setup self.controller_manager = None self.controllers_always_on = [] self.full_controller_sim = (left_arm_sim and right_arm_sim and left_ft_sim and right_ft_sim and left_hand_sim and right_hand_sim and head_sim) if not self.full_controller_sim: # any non-simulation requires ros and the ros_control stack import rospy from ros_control_client_py import (ControllerManagerClient, JointStateClient) if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') # update openrave state from /joint_states self._jointstate_client = JointStateClient( self, topic_name='/joint_states') self.controller_manager = ControllerManagerClient() self.controllers_always_on.append('joint_state_controller') # Convenience attributes for accessing self components. self.left_arm = self.GetManipulator('left') self.right_arm = self.GetManipulator('right') self.head = self.GetManipulator('head') self.left_arm.hand = self.left_arm.GetEndEffector() self.right_arm.hand = self.right_arm.GetEndEffector() self.left_hand = self.left_arm.hand self.right_hand = self.right_arm.hand self.manipulators = [self.left_arm, self.right_arm, self.head] # Dynamically switch to self-specific subclasses. prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, namespace='/left') prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, namespace='/right') prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd') prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm, bhd_namespace='/left', ft_sim=left_ft_sim) prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm, bhd_namespace='/right', ft_sim=right_ft_sim) self.base = HerbBase(sim=segway_sim, robot=self) # Set HERB's acceleration limits. These are not specified in URDF. accel_limits = self.GetDOFAccelerationLimits() accel_limits[self.head.GetArmIndices()] = [ 2. ] * self.head.GetArmDOF() accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF() accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF() self.SetDOFAccelerationLimits(accel_limits) # Determine always-on controllers # hand controllers if not left_hand_sim: self.controllers_always_on.append('left_hand_controller') if not right_hand_sim: self.controllers_always_on.append('right_hand_controller') # force/torque controllers if not left_ft_sim or not right_ft_sim: self.controllers_always_on.append('force_torque_sensor_controller') if not left_ft_sim: self.controllers_always_on.append('left_tare_controller') if not right_ft_sim: self.controllers_always_on.append('right_tare_controller') # Set default manipulator controllers in sim only # NOTE: head is ignored until TODO new Schunk head integrated if left_arm_sim: self.left_arm.sim_controller = self.AttachController(name=self.left_arm.GetName(), args='IdealController', dof_indices=self.left_arm.GetArmIndices(), affine_dofs=0, simulated=True) if right_arm_sim: self.right_arm.sim_controller = self.AttachController(name=self.right_arm.GetName(), args='IdealController', dof_indices=self.right_arm.GetArmIndices(), affine_dofs=0, simulated=True) # load and activate initial controllers if self.controller_manager is not None: self.controller_manager.request( self.controllers_always_on).switch() # Support for named configurations. import os.path self.configurations.add_group('left_arm', self.left_arm.GetArmIndices()) self.configurations.add_group('right_arm', self.right_arm.GetArmIndices()) self.configurations.add_group('head', self.head.GetArmIndices()) self.configurations.add_group('left_hand', self.left_hand.GetIndices()) self.configurations.add_group('right_hand', self.right_hand.GetIndices()) configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml') try: self.configurations.load_yaml(configurations_path) except IOError as e: raise ValueError('Failed laoding named configurations from "{:s}".'.format( configurations_path)) # Hand configurations from prpy.named_config import ConfigurationLibrary for hand in [ self.left_hand, self.right_hand ]: hand.configurations = ConfigurationLibrary() hand.configurations.add_group('hand', hand.GetIndices()) if isinstance(hand, BarrettHand): hand_configs_path = FindCatkinResource('herbpy', 'config/barrett_preshapes.yaml') try: hand.configurations.load_yaml(hand_configs_path) except IOError as e: raise ValueError('Failed loading named hand configurations from "{:s}".'.format( hand_configs_path)) else: logger.warning('Unrecognized hand class. Not loading named configurations.') # Initialize a default planning pipeline. from prpy.planning import ( FirstSupported, MethodMask, Ranked, Sequence, ) from prpy.planning import ( CBiRRTPlanner, CHOMPPlanner, GreedyIKPlanner, IKPlanner, NamedPlanner, SBPLPlanner, SnapPlanner, TSRPlanner, VectorFieldPlanner ) # TODO: These should be meta-planners. self.named_planner = NamedPlanner() self.ik_planner = IKPlanner() # Special-purpose planners. self.snap_planner = SnapPlanner() self.vectorfield_planner = VectorFieldPlanner() self.greedyik_planner = GreedyIKPlanner() # General-purpose planners. self.cbirrt_planner = CBiRRTPlanner() # Trajectory optimizer. try: from or_trajopt import TrajoptPlanner self.trajopt_planner = TrajoptPlanner() except ImportError: self.trajopt_planner = None logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt' ' package in your workspace and built?') try: self.chomp_planner = CHOMPPlanner() except UnsupportedPlanningError: self.chomp_planner = None logger.warning('Failed loading the CHOMP module. Is the or_cdchomp' ' package in your workspace and built?') if not (self.trajopt_planner or self.chomp_planner): raise PrPyException('Unable to load both CHOMP and TrajOpt. At' ' least one of these packages is required.') actual_planner = Sequence( # First, try the straight-line trajectory. self.snap_planner, # Then, try a few simple (and fast!) heuristics. self.vectorfield_planner, #self.greedyik_planner, # Next, try a trajectory optimizer. self.trajopt_planner or self.chomp_planner ) self.planner = FirstSupported( Sequence(actual_planner, TSRPlanner(delegate_planner=actual_planner), self.cbirrt_planner), # Special purpose meta-planner. NamedPlanner(delegate_planner=actual_planner), ) from prpy.planning.retimer import HauserParabolicSmoother self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True) self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False, blend_iterations=5, blend_radius=0.4) self.simplifier = None # Base planning from prpy.util import FindCatkinResource planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml') self.sbpl_planner = SBPLPlanner() try: with open(planner_parameters_path, 'rb') as config_file: import yaml params_yaml = yaml.load(config_file) self.sbpl_planner.SetPlannerParameters(params_yaml) except IOError as e: raise ValueError('Failed loading base planner parameters from "{:s}".'.format( planner_parameters_path)) self.base_planner = self.sbpl_planner # Create action library from prpy.action import ActionLibrary self.actions = ActionLibrary() # Register default actions and TSRs import herbpy.action import herbpy.tsr # Setting necessary sim flags self.talker_simulated = talker_sim self.segway_sim = segway_sim # Set up perception self.detector=None if perception_sim: from prpy.perception import SimulatedPerceptionModule self.detector = SimulatedPerceptionModule() else: from prpy.perception import ApriltagsModule try: kinbody_path = prpy.util.FindCatkinResource('pr_ordata', 'data/objects') marker_data_path = prpy.util.FindCatkinResource('pr_ordata', 'data/objects/tag_data.json') self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array', marker_data_path=marker_data_path, kinbody_path=kinbody_path, detection_frame='head/kinect2_rgb_optical_frame', destination_frame='herb_base', reference_link=self.GetLink('/herb_base')) except IOError as e: logger.warning('Failed to find required resource path. ' \ 'pr_ordata package cannot be found. ' \ 'Perception detector will not be loaded.' \ '\n{}'.format(e)) if not self.talker_simulated: # Initialize herbpy ROS Node import rospy if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') import talker.msg from actionlib import SimpleActionClient self._say_action_client = SimpleActionClient('say', talker.msg.SayAction) def CloneBindings(self, parent): from prpy import Cloned super(HERBRobot, self).CloneBindings(parent) self.left_arm = Cloned(parent.left_arm) self.right_arm = Cloned(parent.right_arm) self.head = Cloned(parent.head) self.left_arm.hand = Cloned(parent.left_arm.GetEndEffector()) self.right_arm.hand = Cloned(parent.right_arm.GetEndEffector()) self.left_hand = self.left_arm.hand self.right_hand = self.right_arm.hand self.manipulators = [ self.left_arm, self.right_arm, self.head ] self.planner = parent.planner self.base_planner = parent.base_planner def _ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwargs): if defer is not False: raise RuntimeError('defer functionality was deprecated in ' 'personalrobotics/prpy#278') # Don't execute trajectories that don't have at least one waypoint. if traj.GetNumWaypoints() <= 0: raise ValueError('Trajectory must contain at least one waypoint.') # Check if this trajectory contains both affine and joint DOFs cspec = traj.GetConfigurationSpecification() needs_base = prpy.util.HasAffineDOFs(cspec) needs_joints = prpy.util.HasJointDOFs(cspec) if needs_base and needs_joints: raise ValueError('Trajectories with affine and joint DOFs are not supported') # Check that the current configuration of the robot matches the # initial configuration specified by the trajectory. if not prpy.util.IsAtTrajectoryStart(self, traj): raise TrajectoryNotExecutable( 'Trajectory started from different configuration than robot.') # If there was only one waypoint, at this point we are done! if traj.GetNumWaypoints() == 1: return traj # Verify that the trajectory is timed by checking whether the first # waypoint has a valid deltatime value. if not prpy.util.IsTimedTrajectory(traj): raise ValueError('Trajectory cannot be executed, it is not timed.') # Verify that the trajectory has non-zero duration. if traj.GetDuration() <= 0.0: logger.warning('Executing zero-length trajectory. Please update the' ' function that produced this trajectory to return a' ' single-waypoint trajectory.', FutureWarning) traj_manipulators = self.GetTrajectoryManipulators(traj) controllers_manip = [] active_controllers = [] if self.head in traj_manipulators: # TODO head after Schunk integration if len(traj_manipulators) == 1: raise NotImplementedError('The head is currently disabled under ros_control.') else: logger.warning('The head is currently disabled under ros_control.') # logic to determine which controllers are needed if (self.right_arm in traj_manipulators and not self.right_arm.IsSimulated() and self.left_arm in traj_manipulators and not self.left_arm.IsSimulated()): controllers_manip.append('bimanual_trajectory_controller') else: if self.right_arm in traj_manipulators: if not self.right_arm.IsSimulated(): controllers_manip.append('right_trajectory_controller') else: active_controllers.append(self.right_arm.sim_controller) if self.left_arm in traj_manipulators: if not self.left_arm.IsSimulated(): controllers_manip.append('left_trajectory_controller') else: active_controllers.append(self.left_arm.sim_controller) # load and activate controllers if not self.full_controller_sim: self.controller_manager.request(controllers_manip).switch() # repeat logic and actually construct controller clients # now that we've activated them on the robot if 'bimanual_trajectory_controller' in controllers_manip: joints = [] joints.extend(self.right_arm.GetJointNames()) joints.extend(self.left_arm.GetJointNames()) active_controllers.append( RewdOrTrajectoryController(self, '', 'bimanual_trajectory_controller', joints)) else: if 'right_trajectory_controller' in controllers_manip: active_controllers.append( RewdOrTrajectoryController(self, '', 'right_trajectory_controller', self.right_arm.GetJointNames())) if 'left_trajectory_controller' in controllers_manip: active_controllers.append( RewdOrTrajectoryController(self, '', 'left_trajectory_controller', self.left_arm.GetJointNames())) if needs_base: if (hasattr(self, 'base') and hasattr(self.base, 'controller') and self.base.controller is not None): active_controllers.append(self.base.controller) else: logger.warning( 'Trajectory includes the base, but no base controller is' ' available. Is self.base.controller set?') for controller in active_controllers: controller.SetPath(traj) prpy.util.WaitForControllers(active_controllers, timeout=timeout) return traj def ExecuteTrajectory(self, traj, *args, **kwargs): # from prpy.exceptions import TrajectoryAborted value = self._ExecuteTrajectory(traj, *args, **kwargs) # TODO meaningful to do this check here? # if so can be done on value future # # for manipulator in active_manipulators: # status = manipulator.GetTrajectoryStatus() # if status == 'aborted': # raise TrajectoryAborted() return value # Inherit docstring from the parent class. ExecuteTrajectory.__doc__ = Robot.ExecuteTrajectory.__doc__ def SetStiffness(self, stiffness, manip=None): """Set the stiffness of HERB's arms and head. Stiffness False/0 is gravity compensation and stiffness True/(>0) is position control. @param stiffness boolean or numeric value 0.0 to 1.0 """ if (isinstance(stiffness, numbers.Number) and not (0 <= stiffness and stiffness <= 1)): raise Exception('Stiffness must be boolean or numeric in the range [0, 1];' 'got {}.'.format(stiffness)) # TODO head after Schunk integration if manip is self.head: raise NotImplementedError('Head immobilized under ros_control, SetStiffness not available.') new_manip_controllers = [] if stiffness: if not self.left_arm.IsSimulated() and (manip is None or manip is self.left_arm): new_manip_controllers.append('left_joint_group_position_controller') if not self.right_arm.IsSimulated() and (manip is None or manip is self.right_arm): new_manip_controllers.append('right_joint_group_position_controller') else: if not self.left_arm.IsSimulated() and (manip is None or manip is self.left_arm): new_manip_controllers.append( 'left_gravity_compensation_controller') if not self.right_arm.IsSimulated() and (manip is None or manip is self.right_arm): new_manip_controllers.append( 'right_gravity_compensation_controller') if not self.full_controller_sim: self.controller_manager.request(new_manip_controllers).switch() def Say(self, words, block=True): """Speak 'words' using talker action service or espeak locally in simulation""" if self.talker_simulated: import subprocess try: proc = subprocess.Popen(['espeak', '-s', '160', '"{0}"'.format(words)]) if block: proc.wait() except OSError as e: logger.error('Unable to speak. Make sure "espeak" is installed locally.\n%s' % str(e)) else: import talker.msg goal = talker.msg.SayGoal(text=words) self._say_action_client.send_goal(goal) if block: self._say_action_client.wait_for_result()
class Pr2LookAtFace(Action): def __init__(self): super(Pr2LookAtFace, self).__init__() self._client = SimpleActionClient('face_detector_action',FaceDetectorAction) self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction) self._timer = None self._executing = False self._pending_face_goal = False self._pending_head_goal = False def set_duration(self, duration): duration = max(duration, 1.5) super(Pr2LookAtFace, self).set_duration(duration) def to_string(self): return 'look_at_face()' def execute(self): super(Pr2LookAtFace, self).execute() print('Pr2LookAtFace.execute() %d' % self.get_duration()) # delay execution to not run nested in the current stack context self._timer = rospy.Timer(rospy.Duration.from_sec(0.001), self._execute, oneshot=True) def _execute(self, event): self._executing = True self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()), self._preempt, oneshot=True) hgoal = None connected = self._client.wait_for_server(rospy.Duration(1.0)) if connected: while self._executing: fgoal = FaceDetectorGoal() self._pending_face_goal = True self._client.send_goal(fgoal) self._client.wait_for_result() self._pending_face_goal = False f = self._client.get_result() if len(f.face_positions) > 0: closest = -1 closest_dist = 1000 for i in range(len(f.face_positions)): dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y\ + f.face_positions[i].pos.z*f.face_positions[i].pos.z if dist < closest_dist: closest = i closest_dist = dist if closest > -1: hgoal = PointHeadGoal() hgoal.target.header.frame_id = f.face_positions[closest].header.frame_id hgoal.target.point.x = f.face_positions[closest].pos.x hgoal.target.point.y = f.face_positions[closest].pos.y hgoal.target.point.z = f.face_positions[closest].pos.z hgoal.min_duration = rospy.Duration(1.0) if self._executing: hconnected = self._head_client.wait_for_server(rospy.Duration(1.0)) if hconnected and self._executing: self._pending_head_goal = True self._head_client.send_goal(hgoal) # self._head_client.wait_for_result() # self._pending_head_goal = False else: hgoal = PointHeadGoal() hgoal.target.header.frame_id = "base_footprint"; hgoal.target.point.x = 2.0 hgoal.target.point.y = -2.0 hgoal.target.point.z = 1.2 hgoal.min_duration = rospy.Duration(1.0) if self._executing: hconnected = self._head_client.wait_for_server(rospy.Duration(1.0)) if hconnected and self._executing: self._pending_head_goal = True self._head_client.send_goal(hgoal) self._head_client.wait_for_result() self._pending_head_goal = False if self._executing: time.sleep(1.0) self._finished_finding_face() def _preempt(self, event): self._executing = False if self._pending_face_goal: self._client.cancel_goal() if self._pending_head_goal: self._head_client.cancel_goal() self._execute_finished() def _finished_finding_face(self): if self._executing: self._executing = False self._timer.shutdown() self._execute_finished()
class LiftObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording) self.posture_controller = SimpleActionClient("/wubble_gripper_grasp_action", GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient("/move_left_arm", MoveArmAction) self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject) self.action_server = SimpleActionServer( ACTION_NAME, LiftObjectAction, execute_cb=self.lift_object, auto_start=False ) def initialize(self): rospy.loginfo("%s: waiting for wubble_grasp_status service" % ACTION_NAME) rospy.wait_for_service("/wubble_grasp_status") rospy.loginfo("%s: connected to wubble_grasp_status service" % ACTION_NAME) rospy.loginfo("%s: waiting for wubble_gripper_grasp_action" % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo("%s: connected to wubble_gripper_grasp_action" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/start_audio_recording") rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/stop_audio_recording") rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME) 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.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def lift_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_support_surface_name = goal.collision_support_surface_name # disable collisions between grasped object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = GRIPPER_GROUP_NAME collision_operation2.object2 = collision_support_surface_name collision_operation2.operation = CollisionOperation.DISABLE ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # this is a hack to make arm lift an object faster obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.REMOVE obj.object.id = collision_support_surface_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) joint_names = current_state.joint_names joint_positions = current_state.actual.positions start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS] start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit if not move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, start_angles, link_padding=gripper_paddings, collision_operations=ordered_collision_operations, ): self.action_server.set_aborted() return self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, LIFT_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations, ): # check if are still holding an object after lift is done grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr("%s: attempted lift failed" % ACTION_NAME) self.action_server.set_aborted() return
class PointArmService(): def __init__(self): rospy.init_node(NAME + 'server' , anonymous=True) 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.tf = tf.TransformListener() self.arm_client.wait_for_server() self.gripper_client.wait_for_server() self.service = rospy.Service('point_arm_service', ArmPointService, self.handle_point) rospy.loginfo("%s: Ready for pointing", NAME) #handles receiving a PointStamped. Not handling preempts - does client do this? remember to check code def handle_point(self, req): success = False #print info about request rospy.loginfo("%s: recieved position %s %s %s in %s", \ NAME, req.position.point.x, req.position.point.y, req.position.point.z, req.position.header.frame_id); if self.point_at(req.position.header.frame_id, req.position.point.x, req.position.point.y, req.position.point.z): success = True return success #(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position def transform_target_point(self, point): # Cannot use arm_shoulder_pan_link & arm_shoulder_tilt_link as target frame_id # because the angles used by the low-level motors are absolute, and fixed to # the arm_base_link coordinate (not relative to the coordinates of pan & tilt) target_frame = 'arm_base_link' #rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z) # Wait for tf info (time-out in 5 seconds) self.tf.waitForTransform(target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(2.0)) # Transform target point & retrieve the pan angle pan_target = self.tf.transformPoint(target_frame, point) pan_angle = math.atan2(pan_target.point.y, pan_target.point.x) #rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \ # NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle) # Transform target point & retrieve the tilt angle tilt_target = self.tf.transformPoint(target_frame, point) tilt_angle = math.atan2(tilt_target.point.z, math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2))) #rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \ # NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle) return [pan_angle, tilt_angle] #point! modified move_arm + reach_at from move_arm_demo def point_at(self, frame_id, x, y, z): goal = SmartArmGoal() goal.target_point = PointStamped() goal.target_point.header.frame_id = frame_id goal.target_point.point.x = x goal.target_point.point.y = y goal.target_point.point.z = z self.move_gripper (-2,2) target_shoulder = list() target_shoulder = self.transform_target_point(goal.target_point) if (target_shoulder[0] < 1.2 and target_shoulder[0] > -1.2): #bend arm a bit goal.target_joints = [target_shoulder[0], target_shoulder[1] + .2, .3, .5] # Sends the goal to the action server. self.arm_client.send_goal(goal) self.arm_client.wait_for_result(rospy.Duration(2.0)) #straighten arm goal.target_joints = [target_shoulder[0], target_shoulder[1], 0, 0] # Sends the goal to the action server. self.arm_client.send_goal(goal) self.arm_client.wait_for_result() # Return result #something broke here, returning True fix is temporary return self.arm_client.get_result() #motors are too slow in the demo to actually wait for arm to go to cobra position before moving self.go_to_cobra() rospy.sleep(2) position = Point(x, y, z) orientation = Quaternion(w=1.0) # self.move_to('/map', position, orientation, 99) return self.point_at(frame_id, x, y, z) #I stole this from somewhere # goal = ErraticBaseGoal() # goal.target_pose.header.stamp = rospy.Time.now() # goal.target_pose.header.frame_id = frame_id # goal.target_pose.pose.position = position # goal.target_pose.pose.orientation = orientation # goal.vicinity_range = vicinity # self.move_client.send_goal(goal) # self.move_client.wait_for_result() def go_to_cobra(self): goal = SmartArmGoal() goal.target_point = PointStamped() goal.target_point.header.frame_id = 'arm_base_link' goal.target_point.point.x = 0 goal.target_point.point.y = 0 goal.target_point.point.z = 0 goal.target_joints = [0.0, 1.972222, -1.972222, 0.0] self.arm_client.send_goal(goal) self.arm_client.wait_for_result() return self.arm_client.get_result() #stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks #would be cool if could point with a single claw rather than both (or with 'thumb'?) def move_gripper(self, left_finger, right_finger): success = False goal = SmartArmGripperGoal() goal.target_joints = [left_finger, right_finger] self.gripper_client.send_goal(goal) self.gripper_client.wait_for_result() if self.gripper_client.get_result(): success = True return success
class arm_bot_base(): def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z): #global self.speed = Twist() self.tracked_blob = Twist() self.state = FSM_STATES.FSM_LOCALIZE self.obj_pose = MoveBaseGoal() self.goal = MoveBaseGoal() self.blobs_3d = Blobs3d() #params self.desired_z = desired_z #0.001 #0.8 self.KpLin = KpLin #1000.0 self.KdLin = KdLin self.KpRot = KpRot #0.1 self.KdRot = KdRot self.tol_z = tol_z #0.00005 #0.05 self.tol_ang= tol_ang #0.5 self.maxLinSpeed = maxLinSpeed #0.2 self.maxRotSpeed = maxRotSpeed self.max_z = max_z #3.0 self.min_z= min_z #0.0005 #0.2 # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) #subscribtions rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback) rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback) # publications self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity ################# # goal_callback : Run everytime '\arm_bot_base\goal' topic receives message. # # Send a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ################# # blob_callback : Run everytime \blobs_3d receives message. # # Used to set rate of node and calls speed_callback() on every run ################# def blob_callback(self, blobs_3d): self.blobs_3d = blobs_3d self.blob_area = 0 if(len(blobs_3d.blobs)): for blob in blobs_3d.blobs: if (blob.area>self.blob_area): self.blob_area = blob.area self.tracked_blob.linear = self.getPose(blob) rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z)) #self.state = FSM_STATES.FSM_OPTIMIZE else: rospy.loginfo("No blobs detected") #self.state = FSM_STATES.FSM_SEARCH ################# # speed_callback : Pubishes velocity and handles state transitions ################# def speed_callback(self): self.zStable = False # flag to signify desired z satisfied self.angStable = False # flag to signify desired angle satisfied self.moveDuration = rospy.Duration(3) # duration of forward movement print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_LOCALIZE STATE # =============> if self.state == FSM_STATES.FSM_LOCALIZE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transition # if-else statement to ensure error-less view of goal status if (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) self.state = FSM_STATES.FSM_LOCALIZE elif(len(self.blobs_3d.blobs)): # if a blob is seen rospy.loginfo("Object detected! Approaching object...") self.state = FSM_STATES.FSM_OPTIMIZE else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) self.state = FSM_STATES.FSM_NAVIGATE # FSM_NAVIGATE STATE # =============> elif self.state == FSM_STATES.FSM_NAVIGATE: # No action required, simply allow path planner to navigate self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transitions if(len(self.blobs_3d.blobs) ): # if a blob is seen self._move_base.cancel_all_goals() # cancell all nav goals rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) rospy.loginfo("Object detected! Aborting goal and approaching object...") self.state = FSM_STATES.FSM_OPTIMIZE else: self.state = FSM_STATES.FSM_NAVIGATE # FSM_SEARCH STATE # ===============> elif self.state == FSM_STATES.FSM_SEARCH: # Spin to search for object self.speed.linear.x = 0.0 self.speed.angular.z = 0.2 # Next state transitions if (len(self.blobs_3d.blobs)): self.state = FSM_STATES.FSM_OPTIMIZE else: self.state = FSM_STATES.FSM_SEARCH # FSM_OPTIMIZE STATE (PID/PD controller) # =================> elif self.state == FSM_STATES.FSM_OPTIMIZE : rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z)) #self.LinDerivator = 0.0 self.RotDerivator = 0.0 # if statement to check object is with range (TO BE DELETED/AMENDED!) if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 rospy.loginfo("Detected object is either too close or too far away.") else: # Compute distance PD error #self.zError = self.tracked_blob.linear.z - self.desired_z #self.pLinError = self.zError*self.KpLin #self.dLinError = self.KdLin*(self.zError-self.LinDerivator) #self.LinDerivator = self.zError #self.speedPID = self.pLinError + self.dLinError # Cap maximum linear speed #if(math.fabs(self.speedPID)>self.maxLinSpeed): # self.speedPID=self.maxLinSpeed*math.fabs(self.speedPID)/self.speedPID; # Set tolerance in z to signify stable position #if(math.fabs(self.zError)<self.tol_z): # self.speed.linear.x = 0.0 # LinDerivator = 0.0 # self.zStable = True #else: # self.speed.linear.x = self.speedPID # self.zStable = False # Set linear speed to 0 self.speed.linear.x = 0.0 # Compute PD angular error self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y); self.pRotError = self.KpRot*self.angError self.dRotError = self.KdRot*(self.angError-self.RotDerivator) self.RotDerivator = self.angError self.anglePID = self.pRotError + self.dRotError; # Cap maximum rotation speed if(math.fabs(self.anglePID)>self.maxRotSpeed): self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID; # Set tolerance in angle to signify stable orientation if(math.fabs(self.angError) < self.tol_ang): self.speed.angular.z = 0.0; RotDerivator = 0.0 self.angStable = True else: self.speed.angular.z = self.anglePID; self.angStable = False # Debug messages rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError)) # Next state transitions if (self.angStable): # if stable position and orientation have been achieved self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state self.moveStartTime = rospy.Time.now() elif(len(self.blobs_3d.blobs)): # else if (while) blobs are being detected self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state else: self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state # FSM_APPROACH_OBJ STATE # =============> elif self.state == FSM_STATES.FSM_APPROACH_OBJ: self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' self.obj_pose.target_pose.header.stamp = rospy.Time.now() self.obj_pose.target_pose.pose.orientation.w = 1.0 # if-else statements to perform corrections in target pose # if distance is higher than 2.20m (4.5 m gazebo) if ( self.tracked_blob.linear.z > 0.0045): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.004) # set goal to (distance - 2m) self._move_base.send_goal(self.obj_pose) rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) self._move_base.wait_for_result() rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 2m and more than 1.20m (2.5 m gazebo) elif (self.tracked_blob.linear.z > 0.0025): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 1m) self._move_base.send_goal(self.obj_pose) rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) self._move_base.wait_for_result() rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 1m else: self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target self._move_base.send_goal(self.obj_pose) rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) self._move_base.wait_for_result() rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN # if ( self.tracked_blob.linear.z > 0.004): # while(self.tracked_blob.linear.z > 0.003): # pass # self._move_base.cancel_all_goals() # cancell all nav goals # self.state = FSM_STATES.FSM_OPTIMIZE # elif ( self.tracked_blob.linear.z > 0.001): # while(self.tracked_blob.linear.z > 0.001): # pass # self._move_base.cancel_all_goals() # cancell all nav goals # self.state = FSM_STATES.FSM_OPTIMIZE # else: # self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' # self.obj_pose.target_pose.header.stamp = rospy.Time.now() # self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # self.obj_pose.target_pose.pose.orientation.w = 1.0 # self._move_base.send_goal(self.obj_pose) # rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) # self._move_base.wait_for_result() # rospy.loginfo(self._move_base.get_result()) # self.state= FSM_STATES.FSM_SPIN # FSM_STABLE STATE # ===============> elif (self.state == FSM_STATES.FSM_STABLE): # Move forward for desired duration to approach target object if (rospy.Time.now() < self.moveStartTime + self.moveDuration): self.speed.linear.x = 0.2 self.state = FSM_STATES.FSM_STABLE # Once duration has elapsed move to spin state else: self.speed.linear.x = 0.0 self.state = FSM_STATES.FSM_SPIN # FSM_SPIN STATE # =============> elif (self.state == FSM_STATES.FSM_SPIN): # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transitions # -Until arm sees object-! self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) #Only publish velocity when no goal is being pursued or expected if (self.state != FSM_STATES.FSM_NAVIGATE): self.speed_pub.publish(self.speed) ############ # getPose(): Returns pose of received blob relative to the camera. ############ def getPose(self, blob): self.pose = Vector3() self.pose.x = blob.center.x self.pose.y = blob.center.y self.pose.z = blob.center.z return self.pose def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.speed_callback() rate.sleep()
class arm_bot_base(): ########### # __init__: Class instantiator. ########## def __init__(self, KpRot, KdRot, tol_z, tol_ang, maxRotSpeed, max_z, min_z): # global self.speed = Twist() self.tracked_blob = Twist() self.old_tracked_blob = Twist() self.state = FSM_STATES.FSM_LOCALIZE self.obj_pose = MoveBaseGoal() self.goal = MoveBaseGoal() self.base_pose = MoveBaseGoal() self.filtered_blobs_3d = Blobs3d() self.blobs_detected = False self.miss_counter = 0 self.correction_start = 0.0 self.serv_state = String() self.arm_state = String() self.blob_count = 0 self.avg_z = 0.0 self.yaw = 0.0 self.init_yaw = 0.0 self.target_yaw = 0.0 self.RotDerivator = 0.0 self.yawDerivator = 0.0 # params self.KpRot = KpRot self.KdRot = KdRot self.tol_z = tol_z self.tol_ang = tol_ang self.maxRotSpeed = maxRotSpeed self.max_z = max_z self.min_z = min_z # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) # Subscribtions rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback) rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback) rospy.Subscriber('/client_node/serv_state', String, self.serv_state_callback) rospy.Subscriber('/uarm/state', String, self.arm_state_callback) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_callback) # Publications self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity self.target_pose_pub = rospy.Publisher('/arm_bot_base/target_pose', PoseStamped) self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity # Kill start up node to save performance rospy.loginfo('Killing start up node......') subprocess.call('rosnode kill /start_up_node', shell = True) # Set up delay to allow for amcl to startup rospy.loginfo('Starting up processes......') for i in range (9): # 10 second delay rospy.loginfo('Initializing behaviour in %d' %(10-i)) rospy.sleep(rospy.Duration(1)) self.loc_start = rospy.Time.now() ################# # goal_callback : Run everytime '\arm_bot_base\goal' topic receives message # # Sends a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ################# # blob_callback : Run everytime \blobs_3d receives message. Filters blobs depending on their position to exclude blobs outside # # the arena and a certain range. The largest blob is selected and recorded for processing. ################# def blob_callback(self, blobs_3d): self.old_tracked_blob = self.tracked_blob self.filtered_blobs_3d = Blobs3d() # Initialize empty filtered blobs list self.max_blob_area = 0 # reset maximum blob area self.sum_x = 0.0 self.blob_count = 0 # Filter blobs if(len(blobs_3d.blobs)): for blob in blobs_3d.blobs: if (blob.center.y >= 0.000 and blob.center.z <= 0.0012): # if blob is closer than 1.2m and below sensor level self.filtered_blobs_3d.blobs.append(blob) # if filtered list contains blobs if(len(self.filtered_blobs_3d.blobs)): self.blobs_detected = True # signal that a blobs have been detected self.miss_counter = 0 # reset miss counter # Find largest blob and track it for blob in self.filtered_blobs_3d.blobs: self.blob_count = self.blob_count + 1 self.sum_x = self.sum_x + blob.center.x if(blob.area > self.max_blob_area): self.max_blob_area = blob.area self.tracked_blob.linear = self.getPose(blob) self.avg_x = float(self.sum_x / self.blob_count) self.update_avg_z() self.tracked_blob.linear.x = self.avg_x self.tracked_blob.linear.z = self.avg_z self.pub_tracked_blob_tf(self.tracked_blob.linear) # else if filtered list does not contain any blobs else: self.miss_counter += 1 # increment miss count if(self.miss_counter >= 10): # Allow for 10 callbacks before signaling non-detection self.blobs_detected = False self.avg_z = 0 self.tracked_blob = Twist() ###################### # arm_state_callback : Run everytime the arm updates it's state. ###################### def arm_state_callback(self, arm_state): self.arm_state = arm_state ###################### # serv_state_callback : Run everytime the server updates it's state. ###################### def serv_state_callback(self, serv_state): self.serv_state = serv_state if self.serv_state.data == "RESET": self.state = FSM_STATES.FSM_RESET self.reset_time = rospy.Time.now() # Used during initial wait ################# # amcl_callback : Debug callback to compute yaw ################# def amcl_callback(self, robot_pose): self.robot_pose = robot_pose (roll,pitch,self.yaw) = euler_from_quaternion([self.robot_pose.pose.pose.orientation.x, \ self.robot_pose.pose.pose.orientation.y, self.robot_pose.pose.pose.orientation.z, self.robot_pose.pose.pose.orientation.w]) ################# # speed_callback : Pubishes velocity and handles state transitions ################# def speed_callback(self): self.zStable = False # flag to signify desired z satisfied self.angStable = False # flag to signify desired angle satisfied self.sleepDuration = rospy.Duration(3) # duration of forward movement # Print state print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_LOCALIZE STATE # =============> if self.state == FSM_STATES.FSM_LOCALIZE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transition # if-else statement to ensure error-less view of goal status if(rospy.Time.now() < self.loc_start + rospy.Duration(20)): self.state = FSM_STATES.FSM_LOCALIZE else: self.state = FSM_STATES.FSM_IDLE # FSM_IDLE STATE # =============> if self.state == FSM_STATES.FSM_IDLE: # Stay still self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transition # if-else statement to ensure error-less view of goal status if (self._move_base.simple_state == 1): # if goal is detected self.state = FSM_STATES.FSM_NAVIGATE else: self.state = FSM_STATES.FSM_IDLE # FSM_RESET STATE # =============> if self.state == FSM_STATES.FSM_RESET: self.reset_goal = MoveBaseGoal() self.reset_goal.target_pose.header.frame_id = 'map' self.reset_goal.target_pose.header.stamp = rospy.Time.now() self.reset_goal.target_pose.pose.position.x = 0.0 self.reset_goal.target_pose.pose.position.y = -2.0 self.reset_goal.target_pose.pose.position.z = 0.0 self.reset_goal.target_pose.pose.orientation.x = 0.0 self.reset_goal.target_pose.pose.orientation.y = 0.0 self.reset_goal.target_pose.pose.orientation.z = 0.5 self.reset_goal.target_pose.pose.orientation.w = 1.00 self._move_base.send_goal(self.reset_goal) self.goal_state = self._move_base.get_state() rospy.loginfo("Reset signal received by server. Moving back to base...") rospy.loginfo("Goal Status: %f" %self.goal_state) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) try: rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps() except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) self.state = FSM_STATES.FSM_IDLE # FSM_NAVIGATE STATE # =============> elif self.state == FSM_STATES.FSM_NAVIGATE: # No action required, simply allow path planner to navigate self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Print debug info if (self._move_base.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('Search for object failed!') # Next state transitions if(self.blobs_detected): # if a blob is seen rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) rospy.loginfo("Object detected! Aborting goal and approaching object...") self.state = FSM_STATES.FSM_OPTIMIZE else: if(self._move_base.get_state() != 3.0): rospy.loginfo("Navigating to object at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z)) else: rospy.loginfo("Waiting for goal...") rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) self.state = FSM_STATES.FSM_NAVIGATE # FSM_SEARCH STATE # ===============> elif self.state == FSM_STATES.FSM_SEARCH: # Spin to search for object self.speed.linear.x = 0.0 self.speed.angular.z = 1.0 # Next state transitions if (self.blobs_detected): self.state = FSM_STATES.FSM_OPTIMIZE elif (self._move_base.simple_state == 1): # if goal is detected self.state = FSM_STATES.FSM_NAVIGATE else: self.state = FSM_STATES.FSM_SEARCH # FSM_OPTIMIZE STATE (PID/PD controller) # =================> elif self.state == FSM_STATES.FSM_OPTIMIZE : self._move_base.cancel_all_goals() rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z)) self.RotDerivator = 0.0 # if statement to check object is with range (TO BE DELETED/AMENDED!) if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 rospy.loginfo("Distance = %f" %(self.tracked_blob.linear.z)) rospy.loginfo("Detected object is either too close or too far away.") else: # Set linear speed to 0 self.speed.linear.x = 0.0 # Compute PD angular error self.angError = -0.1*math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y); self.pRotError = self.KpRot*self.angError self.dRotError = self.KdRot*(self.angError-self.RotDerivator) self.RotDerivator = self.angError self.anglePID = self.pRotError + self.dRotError; # Cap maximum rotation speed if(math.fabs(self.anglePID)>self.maxRotSpeed): self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID # Set tolerance in angle to signify stable orientation if(math.fabs(self.angError) < self.tol_ang): self.speed.angular.z = 0.0 self.RotDerivator = 0.0 self.angStable = True else: self.speed.angular.z = self.anglePID self.angStable = False # Debug messages rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError)) # Next state transitions if (self.angStable): # if stable position and orientation have been achieved self.goal = MoveBaseGoal() # reset goal if (self.tracked_blob.linear.z > 0.001): # if object is cloaser than 1 m self.state = FSM_STATES.FSM_APPROACH_OBJ else: self.correction_start = rospy.Time.now() self.state = FSM_STATES.FSM_STABLE # move to stable state elif(self.blobs_detected): # else if (while) blobs are being detected self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state else: #self._move_base.send_goal(self.goal) #rospy.sleep(self.sleepDuration) #self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state self.state = FSM_STATES.FSM_SEARCH # FSM_APPROACH_OBJ STATE # =============> elif self.state == FSM_STATES.FSM_APPROACH_OBJ: # Set constants and time in goal pose self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' self.obj_pose.target_pose.header.stamp = rospy.Time.now() self.obj_pose.target_pose.pose.orientation.w = 1.0 # if-else statements to perform corrections in target pose # if distance is less than 2m and more than 1.50m #if (self.tracked_blob.linear.z > 0.0015): # self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m) # self.target_pose_pub.publish(self.obj_pose.target_pose) # self._move_base.send_goal(self.obj_pose) # send goal # rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) # rospy.loginfo('Waiting for result...') # self._move_base.wait_for_result() # if self._move_base.get_state() == GoalStatus.SUCCEEDED: # rospy.loginfo('Goal Reached!') # else: # rospy.loginfo('Goal aborted!') # else if distance is less than 1.50m #else: self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.7 # set goal to final target self.target_pose_pub.publish(self.obj_pose.target_pose) self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') # Next state transitions if (self.blobs_detected): self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle else: self.state = FSM_STATES.FSM_SEARCH # FSM_STABLE STATE # ===============> elif (self.state == FSM_STATES.FSM_STABLE): # Allow 2 seconds for blob distance to saturate and then compute forward movement duration if (rospy.Time.now() < self.correction_start + rospy.Duration(2)): rospy.loginfo('Correction of detection errors...') self.state = FSM_STATES.FSM_STABLE else: self.moveDuration = rospy.Duration((self.tracked_blob.linear.z - 0.00015)/0.0001) rospy.loginfo((self.tracked_blob.linear.z - 0.0002)/0.0001) self.moveStartTime = rospy.Time.now() self.state = FSM_STATES.FSM_FINAL_APPROACH # FSM_FINAL_APPROACH # ==================> elif (self.state == FSM_STATES.FSM_FINAL_APPROACH): # Move forward for desired duration to approach target object if (rospy.Time.now() < self.moveStartTime + self.moveDuration): self.speed.linear.x = 0.1 self.speed.angular.z = 0.0 self.state = FSM_STATES.FSM_FINAL_APPROACH # Once duration has elapsed move to spin state else: self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # FSM_SPIN STATE # =============> elif (self.state == FSM_STATES.FSM_SPIN): # Spin self.speed.linear.x = 0.0 self.speed.angular.z = -1.0 # Next state transitions # -Until arm sees object-! (Current content is temporary) if (self.arm_state.data == 'ALIGN_CAMERA' or self.arm_state.data =='ALIGN_BIN'): # if arm signifies that object has been dropped self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # change to WAIT_FOR_ACTION state elif (self._move_base.simple_state != 2): # if a goal is detected self.state = FSM_STATES.FSM_MOVE_TO_BASE else: self.state = FSM_STATES.FSM_SPIN # FSM_WAIT_FOR_ACTION STATE # =============> elif (self.state == FSM_STATES.FSM_WAIT_FOR_ACTION): # Stop motors self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Print arm state for debug rospy.loginfo(self.arm_state.data) # Next state transitions if ((self.arm_state.data == 'LOCATE_BIN') or (self.arm_state.data == 'SEARCH_OBJ')): self.state = FSM_STATES.FSM_SPIN elif (self.arm_state.data == 'IDLE'): # if arm signifies that object has been dropped self.state = FSM_STATES.FSM_IDLE # change to MOVE_TO_BASE state else: # if object not dropped self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # loop back # FSM_MOVE_TO_BASE STATE # =============> elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE): self.goal_state = self._move_base.get_state() rospy.loginfo("Moving back to base") rospy.loginfo("Goal Status: %f" %self.goal_state) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_NAVIGATE rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) try: rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps() except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) # Print general info self.printINFO() # Publish /arm_bot_base/state self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) # Only publish velocity when no goal is being pursued or expected if (self.state != FSM_STATES.FSM_NAVIGATE): self.speed_pub.publish(self.speed) ############ # getPose(): Returns pose of received blob relative to the camera. ############ def getPose(self, blob): self.pose = Vector3() self.pose.x = blob.center.x self.pose.y = blob.center.y self.pose.z = blob.center.z return self.pose ################# # update_avg_z(): Updates the avg_value of distance z from blob. # # Higher weight is given to blobs received with lower distance than the average, # # and vice versa, such that the closest distance is ultimately tracked. ################# def update_avg_z(self): if (self.old_tracked_blob.linear.z == 0): self.avg_z = self.tracked_blob.z else: if(self.tracked_blob.linear.z < self.avg_z): # Give heigher weight to closer blobs self.avg_z = (self.avg_z + 10*self.tracked_blob.linear.z)/(11) else: self.avg_z = (10*self.avg_z + self.tracked_blob.linear.z)/(11) ######################## # pub_tracked_blob_tf(): Publishes a transform frame for the tracked blob. ####################### def pub_tracked_blob_tf(self, pose): br = tf.TransformBroadcaster() br.sendTransform((1000*pose.z, -1000*pose.x, -1000*pose.y), tf.transformations.quaternion_from_euler(0, 0, 1), rospy.Time.now(), 'tracked_blob', 'camera_depth_frame') ############## # printINFO(): Prints general debug info ############# def printINFO(self): # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) # Print goal status info if (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) # Print blob info if(self.blobs_detected): rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("miss_counter = %f" %(self.miss_counter)) else: rospy.loginfo("No blobs detected") rospy.loginfo("miss_counter = %f" %(self.miss_counter)) # Print yaw (**DEBUG**) rospy.loginfo(self.yaw) rospy.loginfo(self.arm_state.data) ######### # spin(): Provides desired rate for speed_callback() and ensures termination ######## def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.speed_callback() rate.sleep()