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.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): 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): 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): 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()
class OmniPoseFollower(object): def __init__(self): self.server = SimpleActionServer( '/whole_body_controller/refills_finger/follow_joint_trajectory', FollowJointTrajectoryAction, self.execute_cb, auto_start=False) self.state_pub = rospy.Publisher('refills_finger/state', JointTrajectoryControllerState, queue_size=10) self.js_sub = rospy.Subscriber('refills_finger/joint_states', JointState, self.js_cb, queue_size=10) self.server.start() def js_cb(self, data): msg = JointTrajectoryControllerState() msg.joint_names = data.name self.state_pub.publish(msg) def execute_cb(self, data): """ :type data: FollowJointTrajectoryGoal :return: """ self.server.set_succeeded()
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, 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()
class GiveVoucher(object): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self._as = SimpleActionServer(name, GiveVoucherAction, self.execute_cb, auto_start=False) self.logo_app = rospy.get_param("~logo_app", "showmummerlogo-a897b8/behavior_1") client = MongoClient(rospy.get_param("~db_host", "localhost"), int(rospy.get_param("~db_port", 62345))) self.db_name = rospy.get_param("~db_name", "semantic_map") self.db = client[self.db_name] self.collection_name = rospy.get_param("~collection_name", "idea_park") self.semantic_map_name = rospy.get_param("~semantic_map_name") self._as.start() rospy.loginfo("... done") def __call_service(self, srv_name, srv_type, req): while not rospy.is_shutdown(): try: s = rospy.ServiceProxy(srv_name, srv_type) s.wait_for_service(timeout=1.) except rospy.ROSException, rospy.ServiceException: rospy.logwarn( "Could not communicate with '%s' service. Retrying in 1 second." % srv_name) rospy.sleep(1.) else: return s(req)
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, 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): 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): 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): 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): """ 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()
class MoveBase(object): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self.odom_topic = rospy.get_param("~odom_topic", "/naoqi_driver_node/odom") self.listener = tf.TransformListener() self.robot_pose = None rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb) self.target_frame = rospy.get_param("~target_frame", "base_link") # self.pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10) self._as = SimpleActionServer(name, MoveBaseAction, self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("... done") def __call_service(self, srv_name, srv_type, req): while not rospy.is_shutdown(): try: s = rospy.ServiceProxy(srv_name, srv_type) s.wait_for_service(timeout=1.) except rospy.ROSException, rospy.ServiceException: rospy.logwarn( "Could not communicate with '%s' service. Retrying in 1 second." % srv_name) rospy.sleep(1.) else: return s(req)
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, 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, 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): # 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, 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 ## 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): 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, 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, 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, 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): 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()
class PipolFollower(): def __init__(self): rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'") self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction, execute_cb = self.execute_cb, preempt_callback = self.preempt_cb, auto_start = False) rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS) self._as.start() def execute_cb(self, goal): print "goal is: " + str(goal) # helper variables success = True # start executing the action for i in xrange(1, goal.order): # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False break self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1]) # publish the feedback self._as.publish_feedback(self._feedback) # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes r.sleep() if success: self._result.sequence = self._feedback.sequence rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)
class ReadyArmActionServer: def __init__(self): self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.ready_arm_server = SimpleActionServer(ACTION_NAME, ReadyArmAction, execute_cb=self.ready_arm, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME) self.ready_arm_server.start() def ready_arm(self, goal): if self.ready_arm_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.move_arm_client.cancel_goal() self.ready_arm_server.set_preempted() if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, READY_POSITION, collision_operations=goal.collision_operations): self.ready_arm_server.set_succeeded() else: rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME) self.ready_arm_server.set_aborted()
def __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) self.scene = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the 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()
class ScanTableActionServer: """ Scan Table Mock Run this file to have mock to the action '/head_traj_controller/head_scan_snapshot_action '(Scan Table) """ def __init__(self): a_scan_table = {'name': '/head_traj_controller/head_scan_snapshot_action', 'ActionSpec': PointHeadAction, 'execute_cb': self.scan_table_cb, 'auto_start': False} self.s = SimpleActionServer(**a_scan_table) self.s.start() def scan_table_cb(self, req): rospy.loginfo('Scan Table \'/head_traj_controller/head_scan_snapshot_action was called.') self.s.set_succeeded() if bool(random.randint(0, 1)) else self.s.set_aborted()
class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while(not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
def __init__(self): self.object_presence_pressure_threshold = rospy.get_param('object_presence_pressure_threshold', 200.0) self.object_presence_opening_threshold = rospy.get_param('object_presence_opening_threshold', 0.02) gripper_action_name = rospy.get_param('gripper_action_name', 'wubble_gripper_command_action') self.gripper_action_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction) # # while not rospy.is_shutdown(): # try: # self.gripper_action_client.wait_for_server(timeout=rospy.Duration(2.0)) # break # except ROSException as e: # rospy.loginfo('Waiting for %s action' % gripper_action_name) # except: # rospy.logerr('Unexpected error') # raise rospy.loginfo('Using gripper action client on topic %s' % gripper_action_name) query_service_name = rospy.get_param('grasp_query_name', 'wubble_grasp_status') self.query_srv = rospy.Service(query_service_name, GraspStatus, self.process_grasp_status) posture_action_name = rospy.get_param('posture_action_name', 'wubble_gripper_grasp_action') self.action_server = SimpleActionServer(posture_action_name, GraspHandPostureExecutionAction, self.process_grasp_action, False) self.action_server.start() rospy.loginfo('wubble_gripper grasp hand posture action server started on topic %s' % posture_action_name)
def __init__(self): rospy.init_node('motion_service') # Load config config_loader = RobotConfigLoader() try: robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config') except KeyError: rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for ' 'Thor Mang.') robot_config_name = 'thor' config_loader.load_xml_by_name(robot_config_name + '_config.xml') # Create publisher for first target if len(config_loader.targets) > 0: self._motion_publisher = MotionPublisher( config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix) else: rospy.logerr('Robot config needs to contain at least one valid target.') self._motion_data = MotionData(config_loader.robot_config) # Subscriber to start motions via topic self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb) # Create action server self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False) self._action_server.register_goal_callback(self._execute_motion_goal) self._action_server.register_preempt_callback(self._preempt_motion_goal) self._preempted = False
def __init__(self): # Initialize new node rospy.init_node(NAME)#, anonymous=False) #initialize the clients to interface with self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction) self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction) self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction) self.move_client.wait_for_server() self.arm_client.wait_for_server() self.gripper_client.wait_for_server() # Initialize tf listener (remove?) self.tf = tf.TransformListener() # Initialize erratic base action server self.result = SmartArmGraspResult() self.feedback = SmartArmGraspFeedback() self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback) #define the offsets # These offsets were determines expirmentally using the simulator # They were tested using points stamped with /map self.xOffset = 0.025 self.yOffset = 0.0 self.zOffset = 0.12 #.05 # this does work! rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME) rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def __init__(self, name): self.fullname = name self.jointPositions = [] self.jointVelocities = [] self.jointAccelerations = [] self.jointNamesFromConfig = [] for configJoint in configJoints: self.jointNamesFromConfig.append(configJoint['name']) self.jointPositions.append(0.0) self.jointVelocities.append(0.0) self.jointAccelerations.append(0.0) if 'mimic_joints' in configJoint: for mimic in configJoint['mimic_joints']: self.jointNamesFromConfig.append(mimic['name']) self.jointPositions.append(0.0) self.jointVelocities.append(0.0) self.jointAccelerations.append(0.0) self.pointsQueue = [] self.lastTimeFromStart = 0.0 startPositions = [0.0, -0.9, -0.9, 0.0, 0.0, 0.0] startVelocities = [0.5]*6 self.jointPositions[1] = -0.9 dynamixelChain.move_angles_sync(ids[:6], startPositions, startVelocities) # self.joint_state_pub = rospy.Publisher('/joint_states', JointState) self.server = SimpleActionServer(self.fullname, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False) self.server.start()
def __init__(self): # Initialize constants self.JOINTS_COUNT = 5 # Number of joints to manage self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME + 'server', anonymous=True) # Initialize publisher & subscriber for shoulder pan self.shoulder_pan_frame = 'arm_shoulder_tilt_link' self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0) self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64) rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan) rospy.wait_for_message('shoulder_pan_controller/state', JointState) # Initialize publisher & subscriber for arm tilt self.arm_tilt_frame = 'arm_pan_tilt_bracket' self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64) rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt) rospy.wait_for_message('arm_tilt_controller/state', JointState) # Initialize publisher & subscriber for elbow tilt self.elbow_tilt_frame = 'arm_bracket' #self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64) rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt) rospy.wait_for_message('elbow_tilt_controller/state', JointState) # Initialize publisher & subscriber for wrist pan self.wrist_pan_frame = 'wrist_pan_link' self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64) rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan) rospy.wait_for_message('wrist_pan_controller/state', JointState) # Initialize publisher & subscriber for wrist tilt self.wrist_tilt_frame = 'wrist_tilt_link' self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64) rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt) rospy.wait_for_message('wrist_tilt_controller/state', JointState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize joints action server self.result = RobbieArmResult() self.feedback = RobbieArmFeedback() self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback) # Reset arm position rospy.sleep(1) self.reset_arm_position() rospy.loginfo("%s: Ready to accept goals", NAME)
class ReemTabletopManipulationMock(): def __init__(self): a_grasp_target_action = {'name': '/tabletop_grasping_node', 'ActionSpec': GraspTargetAction, 'execute_cb': self.grasp_target_action_cb, 'auto_start': False} self.s = SimpleActionServer(**a_grasp_target_action) self.s.start() def grasp_target_action_cb(self, req): rospy.loginfo('Grasp Target Action \'%s\' /tabletop_grasping_node was called.' % req.appearanceID) res = GraspTargetResult() res.detectionResult = TabletopDetectionResult() res.detectionResult.models = [DatabaseModelPoseList()] res.detectionResult.models[0].model_list = [DatabaseModelPose()]# = [0].model_id = req.databaseID res.detectionResult.models[0].model_list[0].model_id = req.databaseID self.s.set_succeeded(res) if bool(random.randint(0, 1)) else self.s.set_aborted(res)
class BaseRotate(): def __init__(self): self._action_name = 'base_rotate' #initialize base controller topic_name = '/base_controller/command' self._base_publisher = rospy.Publisher(topic_name, Twist) #initialize this client self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def run(self, goal): rospy.loginfo('Rotating base') count = 0 r = rospy.Rate(10) while count < 70: if self._as.is_preempt_requested(): self._as.set_preempted() return twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 1.0) self._base_publisher.publish(twist_msg) count = count + 1 r.sleep() self._as.set_succeeded()
def __init__(self): rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'") self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction, execute_cb = self.execute_cb, preempt_callback = self.preempt_cb, auto_start = False) rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS) self._as.start()
def __init__(self, name): self.fullname = name self.goalAngle = 0.0 self.actualAngle = 0.0 self.failureState = False dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5]) self.server = SimpleActionServer( self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False ) self.server.start()
def __init__(self, name): self.fullname = name self.currentValue = 0.0 gripperTopic = '/arm_1/gripper_controller/position_command' self.jppub = rospy.Publisher(gripperTopic, JointPositions) self.server = SimpleActionServer(self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False) self.server.start()
def __init__(self, name): self.fullname = name self.currentAngle = 0.0 # dynamixelChain.move_angle(7, 0.0, 0.5) dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5]) self.server = SimpleActionServer(self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False) self.server.start()
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.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity) self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64) self.action_server = SimpleActionServer(ACTION_NAME, ShakePitchObjectAction, execute_cb=self.shake_pitch_object, auto_start=False)
def __init__(self): self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482] self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482] self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675] self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919] self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919] self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644] self.v = [0] * len(self.r1) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self._action_name = 'kill' self._tf = tf.TransformListener() #initialize base controller topic_name = '/base_controller/command' self._base_publisher = rospy.Publisher(topic_name, Twist) #Initialize the sound controller self._sound_client = SoundClient() # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.pose = None self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback) #initialize this client self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name)
def __init__(self): self._action_name = 'base_rotate' #initialize base controller topic_name = '/base_controller/command' self._base_publisher = rospy.Publisher(topic_name, Twist) #initialize this client self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name)
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.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, DropObjectAction, execute_cb=self.drop_object, auto_start=False)
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 __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False
class axGripperServer: def __init__(self, name): self.fullname = name self.currentAngle = 0.0 # dynamixelChain.move_angle(7, 0.0, 0.5) dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5]) self.server = SimpleActionServer(self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False) self.server.start() def execute_cb(self, goal): rospy.loginfo(goal) self.currentAngle = goal.command.position dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5]) # dynamixelChain.move_angle(7, 0.1, 0.5) rospy.sleep(0.1) print jPositions[4] attempts = 0 while abs(goal.command.position - jPositions[4]) > 0.1 and attempts < 20: rospy.sleep(0.1) print jPositions[4] attempts += 1 if attempts < 20: self.server.set_succeeded() else: self.server.set_aborted()
class axGripperServer: def __init__(self, name): self.fullname = name self.goalAngle = 0.0 self.actualAngle = 0.0 self.failureState = False dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5]) self.server = SimpleActionServer( self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False ) self.server.start() def execute_cb(self, goal): rospy.loginfo(goal) self.currentAngle = goal.command.position dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5]) attempts = 0 for i in range(10): rospy.sleep(0.1) attempts += 1 # while ... todo: add some condition to check on the actual angle # rospy.sleep(0.1) # print jPositions[4] # attempts += 1 if attempts < 20: self.server.set_succeeded() else: self.server.set_aborted() def checkFailureState(self): if self.failureState: print "I am currently in a failure state."
def __init__(self): self.base_frame = '/base_footprint' self.move_client = SimpleActionClient('move_base', MoveBaseAction) self.move_client.wait_for_server() self.tf = tf.TransformListener() self.result = ErraticBaseResult() self.feedback = ErraticBaseFeedback() self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False) self.server.start() rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self, ip_robot): self.ip_robot = ip_robot # pan tilt self.joint_states = [None, None] self.js_pub = rospy.Publisher('/joint_states', JointState, queue_size=5) self.as_ = SimpleActionServer('/robot_controller', JointTrajectoryAction, auto_start=False, execute_cb=self.goal_cb) self.as_.start() self.go_to_position(pan=0.0, tilt=0.0) rospy.Timer(rospy.Duration(0.02), self.js_pub_cb, oneshot=False) rospy.loginfo("We are started!")
def __init__(self, name, configJoints): self.fullname = name self.jointNames = [] for configJoint in configJoints: self.jointNames.append(configJoint["name"]) self.jointNames = self.jointNames[:4] self.failureState = False self.goalPositions = [0.0, -0.9, 0.0, 0.0] self.goalVelocities = [0.5] * 4 self.move_chain() # todo: add method for checking arm's actual positions self.actualPositions = [0.0, -0.9, 0.0, 0.0] self.server = SimpleActionServer( self.fullname, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False ) self.server.start()