mirror of
https://github.com/Jieshoudaxue/ros_senior.git
synced 2026-08-30 00:50:44 +08:00
jieshoudaxue
49 lines
1.3 KiB
Python
Executable File
49 lines
1.3 KiB
Python
Executable File
#! /usr/bin/env python3
|
|
|
|
import rospy
|
|
import actionlib
|
|
from action_sample.msg import DoDishesAction, DoDishesGoal, DoDishesFeedback, DoDishesResult
|
|
|
|
class DishWasherServer(object):
|
|
def __init__(self, action_name):
|
|
self._action_name = action_name
|
|
self._server = actionlib.SimpleActionServer(self._action_name, DoDishesAction, execute_cb=self.execute_cb, auto_start=False)
|
|
|
|
def start_server(self):
|
|
self._server.start()
|
|
|
|
def execute_cb(self, goal):
|
|
rate = rospy.Rate(1)
|
|
|
|
rospy.loginfo("dishwasher %d is working" %goal.dishwasher_id)
|
|
|
|
feedback = DoDishesFeedback()
|
|
result = DoDishesResult()
|
|
|
|
for i in range(10):
|
|
feedback.percent_complete = i * 10
|
|
self._server.publish_feedback(feedback)
|
|
rate.sleep()
|
|
|
|
if self._server.is_preempt_requested():
|
|
rospy.loginfo("the goal is canceled")
|
|
result.total_dishes_cleaned = i * 10
|
|
self._server.set_preempted(result)
|
|
return
|
|
|
|
rospy.loginfo("dishwasher %d finish working" %goal.dishwasher_id)
|
|
result.total_dishes_cleaned = (i+1) * 10
|
|
self._server.set_succeeded(result)
|
|
|
|
|
|
def main():
|
|
rospy.init_node("do_dishes_server")
|
|
|
|
dish_washer_server = DishWasherServer("do_dishes")
|
|
dish_washer_server.start_server()
|
|
|
|
rospy.spin()
|
|
|
|
if __name__ == "__main__":
|
|
main()
|