Python actionlib.SimpleActionServer() Examples

def __init__(self):
        rospy.init_node('gripper_controller')
        self.msg = None
       
       
        self.r_pub = rospy.Publisher('gripper_controller/command',
                Float64,
                queue_size=10)
        """
        self.t_pub = rospy.Publisher('arm_controller/command',
                JointTrajectory,
                queue_size=10)
        self.js_sub = rospy.Subscriber('joint_states', JointState,
                self._jointStateCb)
        """

        # subscribe to command and then spin
        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin() 

def __init__(self, ros_command_interface):

        self.ros_command_interface = ros_command_interface

        self.server = actionlib.SimpleActionServer(
            'niryo_one/tool_action', ToolAction, self.tool_on_goal, False)

        self.change_tool_server = rospy.Service(
            'niryo_one/change_tool', SetInt, self.callback_change_tool)

        self.current_tool_id_publisher = rospy.Publisher(
            '/niryo_one/current_tool_id', Int32, queue_size=1)

        rospy.Timer(rospy.Duration(1 / 1.0), self.publish_current_tool_id)

        self.current_tool = None
        self.available_tools = None
        self.command_list = None
        self.create_tools() 

def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing"); 

def __init__(self):
        self._ns = "/robot/gripper_action_server"

        if demo_constants.is_real_robot():
            self._gripper = PSGGripper() # intera_interface.Gripper()
        else:
            self._gripper = intera_interface.Gripper()

        # Action Server
        self._server = actionlib.SimpleActionServer(
            self._ns,
            GripperCommandAction,
            execute_cb=self._on_gripper_action,
            auto_start=False)
        self._action_name = rospy.get_name()
        self._server.start()
        self._gripper.set_dead_zone("0.021")

        # Action Feedback/Result
        self.feedback_msg = GripperCommandFeedback()
        self.result_msg = GripperCommandResult()

        # Initialize Parameters
        self._timeout = 5.0 

def __init__( self ):

        #Initialisation
        NaoqiNode.__init__( self, self.NODE_NAME )

        #Proxy to interface with LEDs
        self.proxy = self.get_proxy( "ALLeds" )

        #Seed python's random number generator
        random.seed( rospy.Time.now().to_nsec() )

        #Create a subscriber for the fade_rgb topic
        self.subscriber = rospy.Subscriber(
            "fade_rgb",
            FadeRGB,
            self.fade_rgb)

        #Prepare and start actionlib server
        self.actionlib = actionlib.SimpleActionServer(
            "blink",
            BlinkAction,
            self.run_blink,
            False
            )
        self.actionlib.start() 

def __init__(self):
		self._behavior_started = False
		self._preempt_requested = False
		self._current_state = None
		self._active_behavior_id = None

		self._pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=100)
		self._preempt_pub = rospy.Publisher('flexbe/command/preempt', Empty, queue_size=100)
		self._status_sub = rospy.Subscriber('flexbe/status', BEStatus, self._status_cb)
		self._state_sub = rospy.Subscriber('flexbe/behavior_update', String, self._state_cb)

		self._as = actionlib.SimpleActionServer('flexbe/execute_behavior', BehaviorExecutionAction, None, False)
		self._as.register_preempt_callback(self._preempt_cb)
		self._as.register_goal_callback(self._goal_cb)

		self._rp = RosPack()
		self._behavior_lib = BehaviorLibrary()

		# start action server after all member variables have been initialized
		self._as.start()

		rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) 

def __init__(self):
        self._pub = rospy.Publisher('/mobile_base/commands/velocity', Twist,
                                    queue_size=10)
        self._sub = rospy.Subscriber('/mobile_base/events/bumper',
                                     BumperEvent, self.bumper_callback, queue_size=1)
        self._max_vel = rospy.get_param('~max_vel', 0.5)
        self._action_server = actionlib.SimpleActionServer(
            'bumper_action', GoUntilBumperAction, 
            execute_cb=self.go_until_bumper, auto_start=False)
        self._hit_bumper = False
        self._action_server.start() 

def __init__(self):
        NaoqiNode.__init__(self, 'nao_footsteps')

        self.connectNaoQi()

        # initial stiffness (defaults to 0 so it doesn't strain the robot when
        # no teleoperation is running)
        # set to 1.0 if you want to control the robot immediately
        initStiffness = rospy.get_param('~init_stiffness', 0.0)

        # TODO: parameterize
        if initStiffness > 0.0 and initStiffness <= 1.0:
            self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)

        # last: ROS subscriptions (after all vars are initialized)
        rospy.Subscriber("footstep", StepTarget, self.handleStep, queue_size=50)

        # ROS services (blocking functions)
        self.stepToSrv = rospy.Service("footstep_srv", StepTargetService,
                                       self.handleStepSrv)
        self.clipSrv = rospy.Service("clip_footstep_srv", ClipFootstep,
                                     self.handleClipSrv)

    	# Initialize action server
        self.actionServer = actionlib.SimpleActionServer(
            "footsteps_execution",
            ExecFootstepsAction,
            execute_cb=self.footstepsExecutionCallback,
            auto_start=False)
        self.actionServer.start()

        rospy.loginfo("nao_footsteps initialized") 

def __init__( self ):

        #Initialisation
        NaoqiNode.__init__( self, self.NODE_NAME )

        #We need this variable to be able to call stop behavior when preempted
        self.behavior = None
        self.lock = threading.RLock()

        #Proxy for listingBehaviors and stopping them
        self.behaviorProxy = self.get_proxy( "ALBehaviorManager" )

        # Register ROS services
        self.getInstalledBehaviorsService = rospy.Service(
            "get_installed_behaviors",
            GetInstalledBehaviors,
            self.getInstalledBehaviors
            )

        #Prepare and start actionlib server
        self.actionlibServer = actionlib.SimpleActionServer(
            "run_behavior",
            RunBehaviorAction,
            self.runBehavior,
            False
            )

        self.actionlibServer.register_preempt_callback( self.stopBehavior )

        self.actionlibServer.start() 

def __init__(self, name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)
        self._as.start() 

def test_action_client(self):
        t1 = '/action_1'
        server = None

        def execute_cb(goal):
            rospy.sleep(.5)
            if server.is_preempt_requested():
                server.set_preempted()
            else:
                server.set_succeeded(BehaviorExecutionResult(outcome='ok'))

        server = actionlib.SimpleActionServer(t1, BehaviorExecutionAction, execute_cb, auto_start=False)
        server.start()

        client = ProxyActionClient({t1: BehaviorExecutionAction})
        self.assertFalse(client.has_result(t1))
        client.send_goal(t1, BehaviorExecutionGoal())

        rate = rospy.Rate(20)
        for i in range(20):
            self.assertTrue(client.is_active(t1) or client.has_result(t1))
            rate.sleep()
        self.assertTrue(client.has_result(t1))

        result = client.get_result(t1)
        self.assertEqual(result.outcome, 'ok')

        client.send_goal(t1, BehaviorExecutionGoal())
        rospy.sleep(.1)
        client.cancel(t1)
        rospy.sleep(.9)

        self.assertFalse(client.is_active(t1))

        self.assertFalse(client.is_available('/not_there'))
        client = ProxyActionClient({'/invalid': BehaviorExecutionAction}, wait_duration=.1)
        self.assertFalse(client.is_available('/invalid')) 

def __init__(self, node_name, action_name):
        rospy.init_node(node_name)
        self.server = actionlib.SimpleActionServer(action_name, TalkAction,
          self.do_talk, False)
        self.engine = pyttsx.init()
        self.engine_thread = threading.Thread(target=self.loop)
        self.engine_thread.start()
        self.engine.setProperty('volume', rospy.get_param('~volume', 1.0))
        self.engine.setProperty('rate', rospy.get_param('~rate', 200.0))
        self.preempt = rospy.get_param('~preempt', False)
        self.server.start() 

def start(self):
        self.running = True
        
        self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', JointTrajectory, self.process_command)
        self.state_pub = rospy.Publisher(self.controller_namespace + '/state', FollowJointTrajectoryFeedback, queue_size=1)
        self.action_server = actionlib.SimpleActionServer(self.controller_namespace + '/follow_joint_trajectory',
                                                          FollowJointTrajectoryAction,
                                                          execute_cb=self.process_follow_trajectory,
                                                          auto_start=False)
        self.action_server.start()
        Thread(target=self.update_state).start() 

def __init__(self):
        self._pub = rospy.Publisher('/mobile_base/commands/velocity', Twist,
                                    queue_size=10)
        self._sub = rospy.Subscriber('/mobile_base/events/bumper',
                                     BumperEvent, self.bumper_callback, queue_size=1)
        self._max_vel = rospy.get_param('~max_vel', 0.5)
        self._action_server = actionlib.SimpleActionServer(
            'bumper_action', GoUntilBumperAction, 
            execute_cb=self.go_until_bumper, auto_start=False)
        self._hit_bumper = False
        self._action_server.start() 

def __init__(self, rate=100.0):
        self._alive = False
        self.init_success = False

        self._action_name = rospy.get_name()
        # Action Feedback/Result

        """
        Define the joint names
        """
        self._joint_names = ['linear_joint']

        """
        Controller parameters from arguments, messages, and dynamic
        reconfigure
        """
        self._trajectory_control_rate = rate  # Hz
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()
        self._estop_delay = 0

        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()        
        #self._dyn = reconfig_server
        self._ns = '/movo/torso_controller'
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)            
        self._alive = True
        self.estop = False
        self._movo_status_sub = rospy.Subscriber("/movo/feedback/status",Status,self._update_movo_status)
        self._js = rospy.wait_for_message("/movo/linear_actuator/joint_states",JointState)
        self.pos_targets = self._get_current_position(self._joint_names)
        self._sub = rospy.Subscriber("/movo/linear_actuator/joint_states",JointState,self._update_movo_joint_states)
        self._cmd_pub=rospy.Publisher("/movo/linear_actuator_cmd",LinearActuatorCmd,queue_size=10)
        self._server.start()
        
        self.init_success=True 

def __init__(self, rate=100.0):
        self._alive = False
        self.init_success = False

        self._action_name = rospy.get_name()
        # Action Feedback/Result

        """
        Define the joint names
        """
        self._joint_names = ['pan_joint','tilt_joint']

        """
        Controller parameters from arguments, messages, and dynamic
        reconfigure
        """
        self._trajectory_control_rate = rate  # Hz
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()
        self._traj_smoother = TrajectorySmoother(rospy.get_name(),"head")
        self._estop_delay = 0

        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()        
        #self._dyn = reconfig_server
        self._ns = '/movo/head_controller'
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)            
        self._alive = True
        self.estop = False
        self._movo_status_sub = rospy.Subscriber("/movo/feedback/status",Status,self._update_movo_status)
        self._js = rospy.wait_for_message("/movo/head/joint_states",JointState)
        self.pos_targets = self._get_current_position(self._joint_names)
        self._sub = rospy.Subscriber("/movo/head/joint_states",JointState,self._update_movo_joint_states)
        self._cmd_pub=rospy.Publisher("/movo/head/cmd",PanTiltCmd,queue_size=10)
        self._server.start()
        
        self.init_success=True 

def __init__( self, moduleName ):
        # ROS Initialisation
        NaoqiNode.__init__(self, Constants.NODE_NAME )

        # NAOQi Module initialization
        self.moduleName = moduleName
        # Causes ALBroker to fill in ip and find a unused port
        self.ip = ""
        self.port = 0
        self.init_almodule()

        # Used for speech with feedback mode only
        self.speech_with_feedback_flag = False

        # State variables
        self.conf = None

        # Get Audio proxies
        # Speech-recognition wrapper will be lazily initialized
        self.srw = None

        # Subscription to the Proxy events
        self.subscribe()

        # Start reconfigure server
        self.reconf_server = ReConfServer(NodeConfig, self.reconfigure)
        # Client for receiving the new information
        self.reconf_client = dynamic_reconfigure.client.Client(rospy.get_name())

        #Subscribe to speech topic
        self.sub = rospy.Subscriber("speech", String, self.say )

        # Advertise word recognise topic
        self.pub = rospy.Publisher("word_recognized", WordRecognized )

        # Register ROS services
        self.start_srv = rospy.Service(
            "start_recognition",
            Empty,
            self.start )

        self.stop_srv = rospy.Service(
            "stop_recognition",
            Empty,
            self.stop )

        # Actionlib server for altering the speech recognition vocabulary
        self.setSpeechVocabularyServer = actionlib.SimpleActionServer("speech_vocabulary_action", SetSpeechVocabularyAction,
                                                                  execute_cb=self.executeSpeechVocabularyAction,
                                                                  auto_start=False)

        # Actionlib server for having speech with feedback
        self.speechWithFeedbackServer = actionlib.SimpleActionServer("speech_action", SpeechWithFeedbackAction,
                                                                  execute_cb=self.executeSpeechWithFeedbackAction,
                                                                  auto_start=False)
        # Start both actionlib servers
        self.setSpeechVocabularyServer.start()
        self.speechWithFeedbackServer.start() 

def __init__(self, odom_topic, ref_topic, move_topic, path_topic, tree_topic,
                 goal_topic, focus_topic, effort_topic, ogrid_topic, ogrid_threshold):
        """
        Initialize with topic names and ogrid threshold as applicable.
        Defaults are generated at the ROS params level.

        """
        # One-time initializations
        self.revisit_period = 0.05  # s
        self.ogrid = None
        self.ogrid_threshold = float(ogrid_threshold)
        self.state = None
        self.tracking = None
        self.done = True

        # Lil helpers
        self.rostime = lambda: rospy.Time.now().to_sec()
        self.intup = lambda arr: tuple(np.array(arr, dtype=np.int64))
        self.get_hood = lambda img, row, col: img[row-1:row+2, col-1:col+2]

        # Set-up planners
        self.behaviors_list = [car, boat, escape]
        for behavior in self.behaviors_list:
            behavior.planner.set_system(erf=self.erf)
            behavior.planner.set_runtime(sys_time=self.rostime)
            behavior.planner.constraints.set_feasibility_function(self.is_feasible)

        # Initialize resetable stuff
        self.reset()

        # Subscribers
        rospy.Subscriber(odom_topic, Odometry, self.odom_cb)
        rospy.Subscriber(ogrid_topic, OccupancyGrid, self.ogrid_cb)
        rospy.sleep(0.5)

        # Publishers
        self.ref_pub = rospy.Publisher(ref_topic, Odometry, queue_size=1)
        self.path_pub = rospy.Publisher(path_topic, PoseArray, queue_size=3)
        self.tree_pub = rospy.Publisher(tree_topic, PoseArray, queue_size=3)
        self.goal_pub = rospy.Publisher(goal_topic, PoseStamped, queue_size=3)
        self.focus_pub = rospy.Publisher(focus_topic, PointStamped, queue_size=3)
        self.eff_pub = rospy.Publisher(effort_topic, WrenchStamped, queue_size=3)
        self.sampspace_pub = rospy.Publisher(sampspace_topic, PolygonStamped, queue_size=3)
        self.guide_pub = rospy.Publisher(guide_topic, PointStamped, queue_size=3)

        # Actions
        self.move_server = actionlib.SimpleActionServer(move_topic, MoveAction, execute_cb=self.move_cb, auto_start=False)
        self.move_server.start()
        rospy.sleep(1)

        # Timers
        rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
        rospy.Timer(rospy.Duration(self.revisit_period), self.action_check) 

def __init__(self, limb, reconfig_server, rate=100.0,
                 mode='position_w_id', interpolation='minjerk'):
        self._dyn = reconfig_server
        self._ns = 'robot/limb/' + limb
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._action_name = rospy.get_name()
        self._limb = intera_interface.Limb(limb, synchronous_pub=True)
        self._enable = intera_interface.RobotEnable()
        self._name = limb
        self._interpolation = interpolation
        self._cuff = intera_interface.Cuff(limb=limb)
        # Verify joint control mode
        self._mode = mode
        if (self._mode != 'position' and self._mode != 'position_w_id'
            and self._mode != 'velocity'):
            rospy.logerr("%s: Action Server Creation Failed - "
                         "Provided Invalid Joint Control Mode '%s' (Options: "
                         "'position_w_id', 'position', 'velocity')" %
                    (self._action_name, self._mode,))
            return
        self._server.start()
        self._alive = True
        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments, messages, and dynamic
        # reconfigure
        self._control_rate = rate  # Hz
        self._control_joints = []
        self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()

        # Create our PID controllers
        self._pid = dict()
        for joint in self._limb.joint_names():
            self._pid[joint] = intera_control.PID()

        # Create our spline coefficients
        self._coeff = [None] * len(self._limb.joint_names())

        # Set joint state publishing to specified control rate
        self._pub_rate = rospy.Publisher(
            '/robot/joint_state_publish_rate',
             UInt16,
             queue_size=10)
        self._pub_rate.publish(self._control_rate)