def __init__(self): rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Succesfully connected.") rospy.loginfo("Connecting to place AS") self.place_ac = SimpleActionClient('/place', PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Succesfully connected.") self.planning_scene_interface = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the object size self.object_height = rospy.get_param('~object_height') self.object_width = rospy.get_param('~object_width') self.object_depth = rospy.get_param('~object_depth') self.surface_height = rospy.get_param('~surface_height') self.surface_width = rospy.get_param('~surface_width') self.surface_depth = rospy.get_param('~surface_depth') # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param( '~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn( "Didn't find any links to allow contacts... at param ~links_to_allow_contact" ) else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer('/pickup_pose', PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer('/place_pose', PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start()
def __init__(self): self.node_name = "PickAndPlaceServer" rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() # Get the object size self.object_height = 0.1 self.object_width = 0.05 self.object_depth = 0.05 self.pick_pose = rospy.get_param('~pickup_marker_pose') self.place_pose = rospy.get_param('~place_marker_pose') rospy.loginfo("%s: Waiting for pickup action server...", self.node_name) self.pickup_ac = SimpleActionClient('/pickup', PickupAction) connected = self.pickup_ac.wait_for_server(rospy.Duration(3000)) if not connected: rospy.logerr("%s: Could not connect to pickup action server", self.node_name) exit() rospy.loginfo("%s: Connected to pickup action server", self.node_name) rospy.loginfo("%s: Waiting for place action server...", self.node_name) self.place_ac = SimpleActionClient('/place', PlaceAction) if not self.place_ac.wait_for_server(rospy.Duration(3000)): rospy.logerr("%s: Could not connect to place action server", self.node_name) exit() rospy.loginfo("%s: Connected to place action server", self.node_name) rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() self.scene = PlanningSceneInterface() rospy.loginfo("Planning scene created.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact") else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start()
def __init__(self): self.query_shelf_systems_srv = rospy.Service( '~query_shelf_systems', QueryShelfSystems, self.query_shelf_systems_cb) self.query_shelf_layers_srv = rospy.Service('~query_shelf_layers', QueryShelfLayers, self.query_shelf_layers_cb) self.query_facings_srv = rospy.Service('~query_facings', QueryFacings, self.query_facings_cb) self.query_shelf_layer_detection_path_srv = rospy.Service( '~query_detect_shelf_layers_path', QueryDetectShelfLayersPath, self.query_detect_shelf_layers_path_cb) self.query_facing_detection_path_srv = rospy.Service( '~query_detect_facings_path', QueryDetectFacingsPath, self.query_detect_facings_path_cb) self.query_product_counting_path_srv = rospy.Service( '~query_count_products_posture', QueryCountProductsPosture, self.query_count_products_posture_cb) self.finish_perception_srv = rospy.Service('~finish_perception', FinishPerception, self.finish_perception_cb) self.query_reset_beliefstate_srv = rospy.Service( '~reset_beliefstate', Trigger, lambda x: TriggerResponse()) self.detect_shelf_layer_as = SimpleActionServer( '~detect_shelf_layers', DetectShelfLayersAction, execute_cb=self.detect_shelf_layers_cb, auto_start=False) self.detect_shelf_layer_as.register_preempt_callback(self.cancel_cb) self.detect_facings_as = SimpleActionServer( '~detect_facings', DetectFacingsAction, execute_cb=self.detect_facings_cb, auto_start=False) self.detect_facings_as.register_preempt_callback(self.cancel_cb) self.count_products_as = SimpleActionServer( '~count_products', CountProductsAction, execute_cb=self.count_products_cb, auto_start=False) self.count_products_as.register_preempt_callback(self.cancel_cb) self.state_lock = Queue(1) self.detect_shelf_layer_as.start() self.detect_facings_as.start() self.count_products_as.start() self.set_ready(prefix='init') self.set_names()
def __init__(self, name): self.action_name = name self.action_server = SimpleActionServer(self.action_name, FibonacciAction, execute_cb=self.execute_cb, auto_start=False) self.action_server.start()
def __init__(self, height, safe_dist): self.quadrotor_height = height # Height of the motion to the ground self.safe_dist = safe_dist # Distance the drone must maintain from a victim # Create safe_land server self.land_server = SimpleActionServer('safe_land_server', ExecuteLandAction, self.land_Callback, False) self.server_feedback = ExecuteLandFeedback() self.server_result = ExecuteLandResult() # Get client from trajectory server self.trajectory_client = SimpleActionClient( "approach_server", ExecuteDroneApproachAction) self.trajectory_client.wait_for_server() self.point_to_go = ExecuteDroneApproachGoal( ) # Message to define next position to look for victims # quadrotor motion service self.motors = rospy.ServiceProxy('enable_motors', EnableMotors) self.motors.wait_for_service() ## variables self.sonar_me = Condition() self.odometry_me = Condition() # Start trajectory server self.land_server.start()
def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for left finger self.left_finger_frame = 'arm_left_finger_link' self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64) rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger) rospy.wait_for_message('finger_left_controller/state', JointControllerState) # Initialize publisher & subscriber for right finger self.right_finger_frame = 'arm_right_finger_link' self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64) rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger) rospy.wait_for_message('finger_right_controller/state', JointControllerState) # Initialize action server self.result = SmartArmGripperResult() self.feedback = SmartArmGripperFeedback() self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback) # Reset gripper position rospy.sleep(1) self.reset_gripper_position() rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self): 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.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning) self.get_solver_info_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo) self.get_ik_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_ik', GetPositionIK) self.set_planning_scene_diff_client = rospy.ServiceProxy( '/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.find_cluster_bbox = rospy.ServiceProxy( '/find_cluster_bounding_box', FindClusterBoundingBox) 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, PutDownAtAction, execute_cb=self.put_down_at, auto_start=False)
def __init__(self, activate=True): self._ns = rospy.get_namespace() # Read configuration parameters self._fb_rate = read_parameter( self._ns + 'gripper_action_controller/publish_rate', 60.0) self._min_gap = read_parameter( self._ns + 'gripper_action_controller/min_gap', 0.0) self._max_gap = read_parameter( self._ns + 'gripper_action_controller/max_gap', 0.085) self._min_speed = read_parameter( self._ns + 'gripper_action_controller/min_speed', 0.013) self._max_speed = read_parameter( self._ns + 'gripper_action_controller/max_speed', 0.1) self._min_force = read_parameter( self._ns + 'gripper_action_controller/min_force', 40.0) self._max_force = read_parameter( self._ns + 'gripper_action_controller/max_force', 100.0) # Configure and start the action server self._status = CModelStatus() self._name = self._ns + 'gripper_action_controller' self._server = SimpleActionServer(self._name, CModelCommandAction, execute_cb=self._execute_cb, auto_start=False) rospy.Subscriber('status', CModelStatus, self._status_cb) self._cmd_pub = rospy.Publisher('command', CModelCommand, queue_size=3) working = True if activate and not self._ready(): rospy.sleep(2.0) working = self._activate() if not working: return self._server.start() rospy.loginfo(rospy.loginfo('%s: Started' % self._name))
def __init__(self, action_name, **kwargs): rospy.loginfo('broadcasting action server: ' + action_name) # won't use default auto_start=True as recommended here: https://github.com/ros/actionlib/pull/60 self._action_server = SimpleActionServer(action_name, DetectSceneAction, execute_cb=self._execute_cb, auto_start=False) self._initialize(**kwargs) self._action_server.start()
def __init__(self): self.action_server = SimpleActionServer( 'move_base', MoveBaseAction, execute_cb=self.execute_callback, auto_start=False ) # Simple action server will pretend to be move_base # Movement goal state self.goal = None # Is a goal set self.valid_goal = False # Is this a valid goal self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start) self.current_goal_complete = False # Has the Bug algorithm told us it completed self.position = None # move_base feedback reports the current direction self.bugType = bugType.BUG2 # set the bug type to be used # Service for the Bug algorithm to tell us it is done self.bug_done_service = rospy.Service('/move_base_fake/is_bug_done/', SetBool, self.callback_complete) self.subscriber_odometry = rospy.Subscriber( 'odom/', Odometry, self.callback_odometry ) # We need to read the robots current point for the feedback self.subscriber_simple_goal = rospy.Subscriber( '/move_base_simple/goal/', PoseStamped, self.callback_simple_goal ) # Our return goal is done with /move_base_simple/goal/ self.goal_pub = rospy.Publisher( '/move_base/goal/', MoveBaseActionGoal, queue_size=10) # /move_base_simple/goal/ gets published here self.action_server.start()
def __init__(self): # action client self._ac_gp = SimpleActionClient("global_planner_node_action", GlobalPlannerAction) self._ac_gp.wait_for_server() self._ac_lp = SimpleActionClient("local_planner_node_action", LocalPlannerAction) self._ac_lp.wait_for_server() # action servergoal self._as = SimpleActionServer("nav_to_node_action", NavToAction, self.ExecuteCB, auto_start=False) # define goals to send self.global_planner_goal_ = GlobalPlannerGoal() self.local_planner_goal_ = LocalPlannerGoal() self.new_goal_ = False self.new_path_ = False self.global_planner_error_code = -1 self.local_planner_error_code = -1 # new thread running the execution loop self.thread_ = Thread(target=self.Loop) self.thread_.start() self._as.start()
def __init__(self): # Initialize constants self.error_threshold = 0.0175 # Report success if error reaches below threshold self.signal = 1 # Initialize new node rospy.init_node(NAME, anonymous=True) controller_name = rospy.get_param('~controller') # Initialize publisher & subscriber for tilt self.laser_tilt = JointControllerState() self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64) self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal) self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True) rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state) rospy.wait_for_message(controller_name + '/state', JointControllerState) # Initialize tilt action server self.result = HokuyoLaserTiltResult() self.feedback = HokuyoLaserTiltFeedback() self.feedback.tilt_position = self.laser_tilt.process_value self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback) rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self): self._action = SimpleActionServer('averaging', AveragingAction, execute_cb=self.execute_cb, auto_start=False) self._action.register_preempt_callback(self.preempt_cb) self._action.start()
def __init__(self): self._poses = [] self._poses_idx = 0 self._client = SimpleActionClient('/bug2/move_to', MoveToAction) self._action_name = rospy.get_name() + "/execute_path" self._server = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start = False) self._server.start()
def __init__(self): 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.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.interpolated_ik_params_srv = rospy.ServiceProxy( '/l_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams) self.interpolated_ik_srv = rospy.ServiceProxy( '/l_interpolated_ik_motion_plan', GetMotionPlan) self.set_planning_scene_diff_client = rospy.ServiceProxy( '/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_fk_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_fk', GetPositionFK) self.trajectory_filter_srv = rospy.ServiceProxy( '/trajectory_filter_unnormalizer/filter_trajectory', FilterJointTrajectory) self.trajectory_controller = SimpleActionClient( '/l_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PushObjectAction, execute_cb=self.push_object, auto_start=False)
def __init__(self): self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.ready_arm_server = SimpleActionServer(ACTION_NAME, ReadyArmAction, execute_cb=self.ready_arm, auto_start=False)
def __init__(self): """ Make dictionary of dequeues. Subscribe to set of topics defined by the yaml file in directory Stream topics up to a given stream time, dump oldest messages when limit is reached Set up service to bag n seconds of data default to all of available data """ self.successful_subscription_count = 0 # successful subscriptions self.iteration_count = 0 # number of iterations self.streaming = True self.get_params() if len(self.subscriber_list) == 0: rospy.logwarn('No topics selected to subscribe to. Closing.') rospy.signal_shutdown('No topics to subscribe to') return self.make_dicts() self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC, BagOnlineAction, execute_cb=self.start_bagging, auto_start=False) self.subscribe_loop() rospy.loginfo('Remaining Failed Topics: {}\n'.format( self.get_subscriber_list(False))) self._action_server.start()
def __init__(self): server_ip = rospy.get_param('~overpass_server_ip') server_port = rospy.get_param('~overpass_server_port') ref_lat = rospy.get_param('~ref_latitude') ref_lon = rospy.get_param('~ref_longitude') building = rospy.get_param('~building') global_origin = [ref_lat, ref_lon] rospy.loginfo("Server " + server_ip + ":" + str(server_port)) rospy.loginfo("Global origin: " + str(global_origin)) rospy.loginfo("Starting servers...") self.osm_topological_planner_server = SimpleActionServer( '/osm_topological_planner', OSMTopologicalPlannerAction, self._osm_topological_planner, False) self.osm_topological_planner_server.start() osm_bridge = OSMBridge( server_ip=server_ip, server_port=server_port, global_origin=global_origin, coordinate_system="cartesian", debug=False) path_planner = PathPlanner(osm_bridge) path_planner.set_building(building) self.osm_topological_planner_callback = OSMTopologicalPlannerCallback( osm_bridge, path_planner) rospy.loginfo( "OSM topological planner server started. Listening for requests...")
def __init__(self): rospy.loginfo('__init__ start') # create a node rospy.init_node(NODE) # Action server to receive goals self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, self.handle_path, auto_start=False) self.path_server.start() # publishers & clients self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path) # get parameters from launch file self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True) # action server based on use_obstacle_avoidance if self.use_obstacle_avoidance == False: self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction) else: self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction) self.goal_client.wait_for_server() # other fields self.goal_index = 0 self.executePathGoal = None self.executePathResult = ExecutePathResult()
def __init__(self, name): self._server = SimpleActionServer(name, QueryAction, execute_cb=self._execute_cb, auto_start=False) self._server.start() rospy.loginfo('HMI server started on "%s"', name)
def __init__(self, name): rospy.loginfo("Starting {}".format(name)) self._as = SimpleActionServer(name, TopoNavAction, self.execute_cb, auto_start=False) self._as.register_preempt_callback(self.preempt_cb) self.client = SimpleActionClient("/waypoint_nav_node", WayPointNavAction) rospy.loginfo("Waiting for nav server") self.client.wait_for_server() self.nodes = set() self.edges = defaultdict(list) self.distances = {} with open(rospy.get_param("~waypoint_yaml"), 'r') as f: self.waypointInfo = yaml.load(f) for waypoint, info in self.waypointInfo.items(): self.add_node(waypoint) for edge in info["edges"]: self.add_edge(waypoint, edge, 1.0) self.waypointInfo[waypoint]["position"]["x"] *= 0.555 self.waypointInfo[waypoint]["position"]["y"] *= 0.555 self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped, self.pose_cb) self._as.start() rospy.loginfo("Started {}".format(name))
def __init__(self): self.action_server = SimpleActionServer( 'move_base', MoveBaseAction, execute_cb=self.execute_callback, auto_start=False ) # Simple action server will pretend to be move_base # Movement goal state self.goal = None # Is a goal set self.valid_goal = False # Is this a valid goal self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start) self.current_goal_complete = False # Has the Bug algorithm told us it completed self.position = None # move_base feedback reports the current direction ## TO DO!! # Need a service provided by this node or something for the Bug algorithm to tell us it is done # Bug service to start and stop Bug algorithm # Bug service to set a new goal in Bug algorithm # rospy.wait_for_service() # rospy.wait_for_service() self.subscriber_odometry = rospy.Subscriber( 'odom/', Odometry, self.callback_odometry ) # We need to read the robots current point for the feedback self.subscriber_simple_goal = rospy.Subscriber( '/move_base_simple/goal/', PoseStamped, self.callback_simple_goal ) # Our return goal is done with /move_base_simple/goal/ self.goal_pub = rospy.Publisher( '/move_base/goal/', MoveBaseActionGoal, queue_size=10) # /move_base_simple/goal/ gets published here self.action_server.start()
def __init__(self, name="aggressive_gain_buff_action"): self.action_name = name self._as = SimpleActionServer(self.action_name, AggressiveGainBuffAction, execute_cb=self.ExecuteCB, auto_start=False) self.gain_buff_state = GainBuffState.ROUTE self.chassis_state_ = 0 self.sub_odom = rospy.Subscriber("aggressive_buff_info", AggressiveGainBuffInfo, callback=self.OdomCB) self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode", ChassisMode) self.chassis_arrive_state = ChassisArriveState() self.chassis_mode_ = 0 self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction) rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...') ret = self._ac_navto.wait_for_server() rospy.loginfo('GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr('error: NavTo server not started!') self.nav_to_error_code = -1 self.search_start_time = rospy.Time(0) self._ac_look_n_move = SimpleActionClient("look_n_move_node_action", LookAndMoveAction) rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...') ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3)) rospy.loginfo('GAIN_BUFF: Look and Move sever connected!') if ret else rospy.logerr('error: Look and Move server not started!') self.Look_n_move_error_code = -1 self.thread_ = Thread(target=self.Loop) self.thread_.start() self._as.start()
def __init__(self, name): self._action_name = name self._as = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("in action_server")
def __init__(self, namespace, action_spec, execute_cb): self.goal_service = rospy.Service(namespace + '/get_goal_from_id', GetTaskFromID, self.task_id_cb) self.server = SimpleActionServer(namespace, action_spec, execute_cb, auto_start=False) self.server.start()
def __init__(self): self.rate = rospy.Rate(10) self.goto_server = SimpleActionServer('goto', GotoAction, execute_cb=self.goto_execute, auto_start=False) self.goto_server.start() self.last_object = "base1"
def __init__(self): self._default_focus_point = Point(1, 0, 1.05) self._down_focus_point = Point(0.5, 0, 0.5) self._target_focus_point = Point(1, 0, 1.05) self._current_focus_point = Point(1, 0, 1.05) self._current_gaze_action = GazeGoal.LOOK_FORWARD self._prev_gaze_action = self._current_gaze_action self._prev_target_focus_point = array(self._default_focus_point) # Some constants self._no_interrupt = [GazeGoal.NOD, GazeGoal.SHAKE, GazeGoal.LOOK_DOWN] self._nod_positions = [Point(1, 0, 0.70), Point(1, 0, 1.20)] self._shake_positions = [Point(1, 0.2, 1.05), Point(1, -0.2, 1.05)] self._n_nods = 5 self._n_shakes = 5 self._nod_counter = 5 self._shake_counter = 5 self._face_pos = None self._glance_counter = 0 self.gaze_goal_strs = { GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD', GazeGoal.FOLLOW_EE: 'FOLLOW_EE', GazeGoal.GLANCE_EE: 'GLANCE_EE', GazeGoal.NOD: 'NOD', GazeGoal.SHAKE: 'SHAKE', GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE', GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT', GazeGoal.LOOK_DOWN: 'LOOK_DOWN', GazeGoal.NONE: 'NONE', } ## Action client for sending commands to the head. self._head_action_client = SimpleActionClient( '/head_controller/point_head', PointHeadAction) self._head_action_client.wait_for_server() rospy.loginfo('Head action client has responded.') self._head_goal = PointHeadGoal() self._head_goal.target.header.frame_id = 'base_link' self._head_goal.min_duration = rospy.Duration(1.0) self._head_goal.target.point = Point(1, 0, 1) ## Service client for arm states self._tf_listener = TransformListener() ## Server for gaze requested gaze actions self._gaze_action_server = SimpleActionServer( 'gaze_action', GazeAction, self._execute_gaze_action, False) self._gaze_action_server.start() self._is_action_complete = True rospy.Service('get_current_gaze_goal', GetGazeGoal, self._get_gaze_goal)
def __init__(self, action_name, action_type): self.goal_queue = Queue(1) self.result_queue = Queue(1) self.lock = Blackboard().lock self.action_name = action_name self.cancel_next_goal = False self._as = SimpleActionServer(action_name, action_type, execute_cb=self.execute_cb, auto_start=False) self._as.register_preempt_callback(self.cancel_cb) self._as.start()
def __init__(self, action_name, timeout_s=10., **kwargs): rospy.loginfo('broadcasting action server: ' + action_name) self._action_server = SimpleActionServer(action_name, DetectObjectsAction, execute_cb=self._execute_cb, auto_start=False) self._timeout_s = timeout_s self._initialize(**kwargs) self._action_server.start()
def __init__(self): rospy.loginfo("inside __init__") self.Server = "/turtle_thing" print(self.Server) self.turtle_server = SimpleActionServer(self.Server, Turtle_positionAction, execute_cb=self.execute_cb, auto_start=False) self.turtle_server.start()