def main(args=None): global logger rclpy.init(args=args) node = rclpy.create_node('minimal_action_server') logger = node.get_logger() # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently # Default goal callback accepts all goals # Default cancel callback rejects cancel requests action_server = ActionServer( node, Fibonacci, 'fibonacci', execute_callback=execute_callback, cancel_callback=cancel_callback, callback_group=ReentrantCallbackGroup()) # Use a MultiThreadedExecutor to enable processing goals concurrently executor = MultiThreadedExecutor() rclpy.spin(node, executor=executor) action_server.destroy() node.destroy_node() rclpy.shutdown()
def __init__(self): super().__init__('minimal_action_server') self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback)
def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup())
def __init__(self): super().__init__('minimal_publisher') self.publisher_ = self.create_publisher(AdjacencyList, 'graph_node_network', 10) self._action_server = ActionServer(self, Storage, 'storage_request_6', self.storage_callback, goal_callback=self.goal_parse_msg) timer_period = 1 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) self.create_timer(10, self.contents_callback) self.graph = AdjacencyList() self.graph.node = 6 self.graph.adjacent = [3] self.contents = [] print("init.....") print("ready")
def __init__(self, robot, node, jointPrefix, goal_time_tolerance=None): self.robot = robot self.node = node self.previousTime = Time(seconds=robot.getTime()) self.timestep = int(robot.getBasicTimeStep()) # Parse motor and position sensors self.motors = {} self.sensors = {} self.position = {} self.velocity = {} for i in list(range(robot.getNumberOfDevices())): device = robot.getDeviceByIndex(i) if device.getNodeType() in [ Node.LINEAR_MOTOR, Node.ROTATIONAL_MOTOR ]: name = device.getName() positionSensor = device.getPositionSensor() if positionSensor is None: self.node.get_logger().info( 'Motor "%s" doesn\'t have any position \ sensor, impossible to include it in the action server.' ) else: self.motors[jointPrefix + name] = device self.sensors[jointPrefix + name] = positionSensor self.position[jointPrefix + name] = 0.0 self.velocity[jointPrefix + name] = 0.0 positionSensor.enable(self.timestep) self.numberOfMotors = len(self.motors) # Initialize trajectory list and action server self.joint_path_tolerances = [0.05] * self.numberOfMotors self.trajectories = [] self.server = ActionServer( self.node, FollowJointTrajectory, 'follow_joint_trajectory', execute_callback=self.update, goal_callback=self.on_goal, cancel_callback=self.on_cancel, handle_accepted_callback=self.on_goal_accepted)
def __init__(self, node: Node, action_name: str, delay: float, future: Optional[Future] = None): self.__actsrvr = ActionServer( node, Trigger, action_name, execute_callback=self.__execute_callback, cancel_callback=lambda cancel_request: (print(''), node.get_logger( ).info("Canceling goal..."), CancelResponse.ACCEPT)[-1], goal_callback=lambda goal_request: (inprog := self.__goal_handle is not None, self.__node.get_logger( ).info("Rejecting as action allready in progress") if inprog else None, GoalResponse.REJECT if inprog else GoalResponse.ACCEPT)[-1]) self.__node = node self.__future = future self.__delay = delay self.__action_name = action_name
def __init__(self, robot, node, joint_prefix, controller_name): self.__robot = robot self.__node = node self.__timestep = int(robot.getBasicTimeStep()) # Config self.__default_tolerance = 0.05 # Parse motor and position sensors self.__motors = {} for i in list(range(robot.getNumberOfDevices())): device = robot.getDeviceByIndex(i) if device.getNodeType() in [ Node.LINEAR_MOTOR, Node.ROTATIONAL_MOTOR ]: name = device.getName() if device.getPositionSensor() is None: self.__node.get_logger().warn( f'Motor `{name}` doesn\'t have any position sensor.') else: self.__motors[joint_prefix + name] = device device.getPositionSensor().enable(self.__timestep) # Initialize trajectory list and action server self.__current_point_index = 1 self.__start_time = None self.__goal = None self.__mode = None self.__tolerances = {} self.__server = ActionServer( self.__node, FollowJointTrajectory, controller_name + '/follow_joint_trajectory', execute_callback=self.__on_update, goal_callback=self.__on_goal, cancel_callback=self.__on_cancel, handle_accepted_callback=self.__on_goal_accepted)
def __init__(self): super().__init__('calculator') self.argument_a = 0.0 self.argument_b = 0.0 self.argument_operator = 0 self.argument_result = 0.0 self.argument_formula = '' self.operator = ['+', '-', '*', '/'] self.callback_group = ReentrantCallbackGroup() self.declare_parameter('qos_depth', 10) qos_depth = self.get_parameter('qos_depth').value QOS_RKL10V = QoSProfile(reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, depth=qos_depth, durability=QoSDurabilityPolicy.VOLATILE) self.arithmetic_argument_subscriber = self.create_subscription( ArithmeticArgument, 'arithmetic_argument', self.get_arithmetic_argument, QOS_RKL10V, callback_group=self.callback_group) self.arithmetic_service_server = self.create_service( ArithmeticOperator, 'arithmetic_operator', self.get_arithmetic_operator, callback_group=self.callback_group) self.arithmetic_action_server = ActionServer( self, ArithmeticChecker, 'arithmetic_checker', self.execute_checker, callback_group=self.callback_group)
def __init__(self, node, action_name, action_type, callback): self._action_type = action_type self._callback = callback self._server = ActionServer(node, action_type, action_name, self._cb)
class SubtaskNodeBase(Node, metaclass=ABCMeta): """サブタスクを定義する抽象クラス """ def __init__(self): super().__init__(self.node_name()) self._dict = self.init_argument() self.cb_group = ReentrantCallbackGroup() self._action_server = ActionServer( self, TsDoSubtask, "subtask_node_" + str(self.id()), execute_callback=self.execute_callback, callback_group=self.cb_group, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) # self.srv = self.create_service(TsDoTask, \ # "subtask_node_" + str(self.id()), \ # self._service_callback, \ # # self._test_service_callback, \ # callback_group = self.cb_group) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): self.get_logger().info("Received goal request") return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): self.get_logger().info('Received cancel request') self._timer = self.create_timer( 0.0, self._cancel_service_callback, callback_group=ReentrantCallbackGroup()) return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): self.get_logger().info(">> Start") self._dict.update(json.loads(goal_handle.request.arg_json)) result = await self.service_callback(self._dict, TsDoSubtask.Result(), goal_handle) self.get_logger().warning(result.message) if goal_handle.is_cancel_requested: goal_handle.canceled() result.message = "Canceled" elif result.message == "Success": goal_handle.succeed() elif result.message == "Canceled": goal_handle.canceled() else: # Abortとして処理 goal_handle.abort() if result.message != "Success": self.get_logger().warning(f"return {result.message}") return result @abstractmethod def node_name(self) -> str: """ROS2 ノードの名前""" pass @abstractmethod def id(self) -> int: """データベースID""" pass # async def _service_callback(self, request, response): # """引数更新・実行ログ用""" # self.get_logger().info(">> Start") # # 引数を更新 # self._dict.update(json.loads(request.arg_json)) # response = await self.service_callback(self._dict, response) # self.get_logger().info(f">> {response.message}") # return response @abstractmethod async def service_callback(self, request, response, goal_handle) -> "response": """実行時の働き""" pass async def _cancel_service_callback(self): self._timer.cancel() await self.cancel_service_callback() async def cancel_service_callback(self): """キャンセルときに必要な動作がある場合オーバーライドする """ pass def init_argument(self) -> dict: """サブタスクに引数が必要な場合、オーバーライドしてください Returns: dict: タスクの引数をkey、デフォルト値をvalueとして辞書にしたもの """ return {} def _test_service_callback(self, request, response, goal_handle) -> "response": self.get_logger().info("Callback accepted") wait = random.randint(5, 15) self.get_logger().info(f"execute {wait} seconds") time.sleep(wait) self.get_logger().info("success") response.message = "Success" return response
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): """Provides a handle to an accepted goal.""" self.get_logger().info('Deferring execution...') self._goal_handle = goal_handle self._timer = self.create_timer(3.0, self.timer_callback) def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def timer_callback(self): # Execute the defered goal if self._goal_handle is not None: self._goal_handle.execute() self._timer.cancel() async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.set_canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) self.get_logger().info('Publishing feedback: {0}'.format( feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.set_succeeded() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
def __init__(self): super().__init__('attitude_controller') self._enable_attitude_control = False self._leg_names = [ 'front_left', 'middle_left', 'rear_left', 'rear_right', 'middle_right', 'front_right' ] self._leg_args = [ pi / 6.0, pi / 2.0, pi * 5.0 / 6.0, -pi * 5.0 / 6.0, -pi / 2.0, -pi / 6.0 ] self._leg_position_pubs = [] for leg_name in self._leg_names: self._leg_position_pubs.append( self.create_publisher( LegPosition, 'hapthexa/leg/' + leg_name + '/leg_position', 10)) self._attitude_sub = self.create_subscription(Attitude, 'hapthexa/attitude', self.attitude_callback, 10) self._parameter_event_sub = self.create_subscription( ParameterEvent, 'parameter_events', self.parameter_callback, 10) self.declare_parameter("kp", 0.01) self.declare_parameter("ki", 0.0) self.declare_parameter("kd", 0.0) self.declare_parameter("roll_goal", 0.0) self.declare_parameter("pitch_goal", 0.0) self._wakeup_action_server = ActionServer(self, Empty, 'hapthexa/wakeup', self.wakeup_action_callback) self._wakeup_action_server = ActionServer(self, Empty, 'hapthexa/default', self.default_action_callback) self._lie_action_server = ActionServer(self, Empty, 'hapthexa/lie', self.lie_action_callback) self._move_succeed_leg_count = 0 self._move_leg_action_clients = [] for leg_name in self._leg_names: self._move_leg_action_clients.append( ActionClient(self, MoveLeg, 'hapthexa/leg/' + leg_name + '/move_leg')) self._move_leg_args_action_clients = [] for leg_name in self._leg_names: self._move_leg_args_action_clients.append( ActionClient(self, MoveLegArgs, 'hapthexa/leg/' + leg_name + '/move_leg_args')) self._send_goal_future = [0] * 6 self._get_result_future = [0] * 6 # self._get_result_future[1] = rclpy.task.Future() timer_period = 0.01 self._timer = self.create_timer(timer_period, self.timer_callback) self.declare_parameter("enable_pid", True) self._leg_circle_radius = 22.0 self._leg_root_radius = 8.0 self._bias_x = 0.0 self._bias_y = 0.0 self._bias_z = 15.0 self._robot_roll = 0.0 self._robot_pitch = 0.0 self._roll_pid = PID() self._pitch_pid = PID() self._roll_goal = 0.0 self._pitch_goal = 0.0
class MinimalActionServer(Node): """Minimal action server that processes one goal at a time.""" def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes the goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): # If goal is flagged as no longer active (ie. another goal was accepted), # then stop executing if not goal_handle.is_active: self.get_logger().info('Goal aborted') return Fibonacci.Result() if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server_6') self._action_server = ActionServer( self, Simple, 'simple_6', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) self.get_logger().info('SERVER 6 : ONLINE') def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" # This server allows multiple goals in parallel self.get_logger().info('SERVER 6 : Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('SERVER 6 : Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('SERVER 6 : Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Simple.Feedback() feedback_msg.time_passed = [0] # Start executing the action for i in range(1, 5): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('SERVER 6 : Goal canceled') return Simple.Result() # Update Fibonacci sequence feedback_msg.time_passed.append(i) self.get_logger().info('SERVER 6 : Publishing feedback: {0}'.format(feedback_msg.time_passed)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Simple.Result() result.transition = 't8' self.get_logger().info('SERVER 6 : Returning result: {0}'.format(result.transition)) return result
class Supervisor(Node): PUBLISH_PD = 1 # seconds def __init__(self, ns="/l2"): super().__init__('l2_supervisor', namespace=ns) self.get_logger().info("Init") self.state_pub = self.create_publisher(L2SequenceArray, 'state', 10) self.timer = self.create_timer(self.PUBLISH_PD, self.gc_state_callback) self.client = docker.from_env() self.config = {} self.state = [] self.path = "/config/" self.load_config() self.watch_handler = FileSystemEventHandler() self.watch_handler.on_modified = self.load_config self.observer = Observer() self.observer.schedule(self.watch_handler, self.path, recursive=True) self.observer.start() self._action_server = ActionServer( self, L2SequenceAction, 'sequence', execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy_node(self): self._action_server.destroy() super().destroy_node() def load_config(self, event=None): self.get_logger().info("Loading from %s" % self.path) self.config = {} for dirName, subdirList, fileList in os.walk(self.path): for fName in fileList: if os.path.splitext(fName)[1] not in [".yaml", ".yml"]: continue self.get_logger().info("\t...%s" % fName) with open(os.path.join(dirName, fName), 'r') as stream: try: self.config.update(yaml.safe_load(stream)) except yaml.YAMLError as exc: self.get_logger().error(str(exc)) self.get_logger().info("======== Merged Config ========") for (k, v) in self.config.items(): self.get_logger().info(k + ":") for kv2 in v.items(): self.get_logger().info("\t%s: %s" % kv2) self.get_logger().info("======== End Merged Config ========") def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" for item in goal_request.sequence.items: if self.config.get(item.name) is None: self.get_logger().info( 'Rejecting incoming sequence; no config match for item %s' % item.name) return GoalResponse.REJECT self.get_logger().info('Accepting %d new sequence items' % len(goal_request.sequence.items)) return GoalResponse.ACCEPT def cancel_callback(self, goal): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Accepting cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes the goal.""" self.get_logger().info('Adding goal to work queue & executing...') work = Work(goal_handle, self) self.state.append(work) return work.execute() # Blocks thread until execution complete def gc_state_callback(self): seqs = L2SequenceArray() for s in self.state: seqs.sequences.append(L2Sequence(items=s.sequence.items)) self.state_pub.publish(seqs) # clean up state self.state = [s for s in self.state if s.active()]
class MinimalActionServer(Node): """Minimal action server that processes one goal at a time.""" def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accept or reject a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal): """Accept or reject a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Execute the goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): # If goal is flagged as no longer active (ie. another goal was accepted), # then stop executing if not goal_handle.is_active: self.get_logger().info('Goal aborted') return Fibonacci.Result() if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) self.get_logger().info('Publishing feedback: {0}'.format( feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
def test_cancel_defered_goal(self): server_goal_handle = None def handle_accepted_callback(gh): nonlocal server_goal_handle server_goal_handle = gh def cancel_callback(request): return CancelResponse.ACCEPT def execute_callback(gh): # The goal should already be in state CANCELING self.assertTrue(gh.is_cancel_requested) gh.canceled() return Fibonacci.Result() action_server = ActionServer( self.node, Fibonacci, 'fibonacci', callback_group=ReentrantCallbackGroup(), execute_callback=execute_callback, handle_accepted_callback=handle_accepted_callback, cancel_callback=cancel_callback, ) goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) goal_msg = Fibonacci.Impl.SendGoalService.Request() goal_msg.goal_id = goal_uuid goal_future = self.mock_action_client.send_goal(goal_msg) rclpy.spin_until_future_complete(self.node, goal_future, self.executor) send_goal_response = goal_future.result() self.assertTrue(send_goal_response.accepted) self.assertIsNotNone(server_goal_handle) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_ACCEPTED) # Cancel the goal, before execution cancel_srv = CancelGoal.Request() cancel_srv.goal_info.goal_id = goal_uuid cancel_srv.goal_info.stamp.sec = 0 cancel_srv.goal_info.stamp.nanosec = 0 cancel_future = self.mock_action_client.cancel_goal(cancel_srv) rclpy.spin_until_future_complete(self.node, cancel_future, self.executor) cancel_result = cancel_future.result() self.assertEqual(len(cancel_result.goals_canceling), 1) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELING) # Execute the goal server_goal_handle.execute() # Get the result and exepect it to have canceled status get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result = get_result_future.result() self.assertEqual(result.status, GoalStatus.STATUS_CANCELED) self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELED) action_server.destroy()
class RandomCrawlActionServer(Node): def __init__(self): super().__init__('action_server') self.env = TurtlebotEnv() self.action_size = self.env.action_space() print(self.action_size) self.state = self.env.reset() self.observation_space = len(self.state) self.state = np.reshape(self.state, [1, self.observation_space]) pkg_share_directory = get_package_share_directory('nav2_turtlebot3_rl') path = os.path.join(pkg_share_directory, "saved_models/random_crawl_waffle.h5") self.model = load_model(path) q = self.model.predict(self.state) self._goal_handle = None self._goal_lock = threading.Lock() self._action_server = ActionServer( self, RandomCrawl, 'RandomCrawl', execute_callback=self.execute_callback, handle_accepted_callback=self.handle_accepted_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self.env.cleanup() self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal_handle): self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): while not goal_handle.is_cancel_requested and rclpy.ok(): q_values = self.model.predict(self.state) action = np.argmax(q_values) next_state, reward, terminal = self.env.step(action) next_state = np.reshape(next_state, [1, self.observation_space]) self.state = next_state sleep(parameters.LOOP_RATE) goal_handle.succeed() self.env.stop_action() result = RandomCrawl.Result() return result
def __init__(self): super().__init__('ttac3_action_server') self._action_server = ActionServer(self, TTAC3, 'ttac3', self.execute_callback)
class TaskSchedulerManager(Node): """TaskNodeを管理するマネージャノード """ def __init__(self): """コンストラクタ """ self.cb_group = ReentrantCallbackGroup() super().__init__("task_scheduler_manager") self.get_logger().info('task_scheduler_manager') self.action_server = ActionServer(self, TsReq, "tms_ts_master", self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=self.cb_group) def destroy_node(self): """デストラクタ """ self.action_server.destroy() super().destroy_node() def goal_callback(self, goal_handle): """タスク実行要求受付 """ self.get_logger().info('Received Task Request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """タスクのキャンセル """ self.get_logger().info('Received Cancel task') self._cancel_timer = self.create_timer(0.0, self._cancel, callback_group=self.cb_group) return CancelResponse.ACCEPT async def _cancel(self): self._cancel_timer.cancel() await self.goal_handle_client.cancel_goal_async() async def execute_callback(self, goal_handle): """タスク処理 """ global executor self.goal_handle = goal_handle task_id = goal_handle.request.task_id arg_json = goal_handle.request.arg_json if arg_json != '': # 引数が存在するとき self.arg_data = json.loads(arg_json) else: self.arg_data = {} self.get_logger().info(f'Execute task{task_id}, args:{self.arg_data}') task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data) pprint(task_tree) # generate task_node task_node = TaskNode(task_tree) executor.add_node(task_node) # added last added task ## execute client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup()) if not client.wait_for_server(timeout_sec=5.0): self.get_logger().error('No action server available') goal_handle.abort() result = TsReq.Result() result.message = "Abort" return result goal = TsDoTask.Goal() self.goal_handle_client = await client.send_goal_async(goal) if not self.goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsReq.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") self.future_result = await self.goal_handle_client.get_result_async() # 結果を返す msg = self.future_result.result.message if msg == "Success": goal_handle.succeed() result = TsReq.Result() result.message = "Success" else: goal_handle.abort() result = TsReq.Result() result.message = msg # result.message = "Success" # task_node.destroy_node() return result async def convert_task_to_subtasks(self, task_id, arg_data): def func(m): arg_str = m.groups()[0] args = arg_str.split('.') answer = arg_data.copy() for arg in args: answer = answer.get(arg, {}) if answer == {}: self._is_valid_subtask_replace = False return '"ARGUMENT ERROR"' else: return str(answer) subtask_list = [] tmsdb_data = await self.call_dbreader(task_id) # this is list subtask_str = tmsdb_data[0].etcdata self.get_logger().info(f"find task '{tmsdb_data[0].name}'") self.get_logger().info(f"read task '{subtask_str}'") subtask_raw_list = re.findall(r'[0-9]+\$\{.*?\}|[0-9]+|\+|\|', subtask_str) self._is_valid_subtask_replace = True for subtask_raw in subtask_raw_list: subtask = subtask_raw.split("$") generated_subtask = [] for elem in subtask: elem = re.sub(r"\((.*?)\)",\ func,\ elem) generated_subtask.append(elem) subtask_list.append(generated_subtask) if self._is_valid_subtask_replace == False: self.get_logger().warning('task argument error!') return [] # 構文解析 _stack = [] for s in subtask_list: if s == ['+']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['serial', pre2, pre1]) elif s == ['|']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['parallel', pre2, pre1]) else: _stack.append(['subtask', s]) syntax_tree = _stack print(syntax_tree) if len(syntax_tree) != 1: self.get_logger().warning('subtask syntax error!') return [] return syntax_tree[0] async def call_dbreader(self, id): """[tms_db_reader] DBからデータを読み取る """ self.cli_dbreader = self.create_client(TmsdbGetData, 'tms_db_reader', callback_group=self.cb_group) while not self.cli_dbreader.wait_for_service(timeout_sec=1.0): self.get_logger().info( 'service "tms_db_reader" not available, waiting again...') req = TmsdbGetData.Request() req.tmsdb.id = id + 100000 # TODO: why add 100000 ? self.future_dbreader = self.cli_dbreader.call_async(req) await self.future_dbreader if self.future_dbreader.result() is not None: res = self.future_dbreader.result().tmsdb return res else: self.get_logger().info('Service "tms_db_reader" call failed %r' % (self.future_dbreader.exception(), )) async def read_name(self, id): """[tms_db_reader] DBから名前を読み取る """ tmsdb = await self.call_dbreader(int(id)) return tmsdb[0].name
class TaskSchedulerManager(Node): """TaskNodeを管理するマネージャノード """ def __init__(self): self.task_node_list = [] self.cb_group = ReentrantCallbackGroup() self.name = 'task_scheduler_manager' self.goal_handles = [] super().__init__('task_scheduler_manager') self.action_server = ActionServer( self, TsReq, "tms_ts_master", self.execute_callback, callback_group=self.cb_group, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback ) def destroy_node(self): self.action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """タスク実行要求をACCEPT or REJECTする """ self.get_logger().info('Received Task Request') return GoalResponse.ACCEPT def cancel_callback(self,goal_handle): """タスク停止要求をACCEPT or REJECTする """ self.get_logger().info('Received Cancel Task') for goal_h in self.goal_handles: try: goal_h.cancel_goal_async() except rclpy.handle.InvalidHandle as e: print(e) return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """タスク処理 """ global executor task_id = goal_handle.request.task_id data_str = goal_handle.request.arg_json if data_str != '': self.arg_data = json.loads(data_str) else: self.arg_data = {} # syntax analyze task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data) if task_tree == []: # ERROR goal_handle.abort() result = TsReq.Result() result.message = "Syntax Error" return result # generate task_node task_node = TaskNode(task_tree) self.task_node_list.append(task_node) executor.add_node(self.task_node_list[len(self.task_node_list) - 1]) # added last added task ## execute client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup()) client.wait_for_server() goal = TsDoTask.Goal() goal_handle_client = await client.send_goal_async(goal) self.goal_handles.append(goal_handle_client) if not goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsReq.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") self.future_result = await goal_handle_client.get_result_async() #if not self.future_result.done(): if True: goal_handle.abort() result = TsReq.Result() result.message = "Abort" return result # return result goal_handle.succeed() result = TsReq.Result() result.message = "Success" task_node.destroy_node() return result async def convert_task_to_subtasks(self, task_id, arg_data): def func(m): arg_str = m.groups()[0] args = arg_str.split('.') answer = arg_data.copy() for arg in args: answer = answer.get(arg, {}) if answer == {}: self._is_valid_subtask_replace = False return '"ARGUMENT ERROR"' else: return str(answer) subtask_list = [] tmsdb_data = await self.call_dbreader(task_id) # this is list subtask_str = tmsdb_data[0].etcdata print(f"[{self.name}] >> find task '{tmsdb_data[0].name}'") print(f"[{self.name}] >> read task '{subtask_str}'") subtask_raw_list = re.findall(r'[0-9]+\$\{.*?\}|[0-9]+|\+|\|', subtask_str) self._is_valid_subtask_replace = True for subtask_raw in subtask_raw_list: subtask = subtask_raw.split("$") generated_subtask = [] for elem in subtask: elem = re.sub(r"\((.*?)\)",\ func,\ elem) generated_subtask.append(elem) subtask_list.append(generated_subtask) print(f"subtask_list: {subtask_list}") if self._is_valid_subtask_replace == False: print(f"[{self.name}] >> argument error!") return [] # 構文解析 _stack = [] for s in subtask_list: if s == ['+']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['serial', pre2, pre1]) elif s == ['|']: pre1 = _stack.pop(-1) pre2 = _stack.pop(-1) _stack.append(['parallel', pre2, pre1]) else: _stack.append(['subtask', s]) syntax_tree = _stack print(f"[{self.name}] >> analyze task") print(syntax_tree) if len(syntax_tree) != 1: print(f"[{self.name}] >> syntax error!") return [] return syntax_tree[0] async def call_dbreader(self, id): """[tms_db_reader] DBからデータを読み取る """ self.cli_dbreader = self.create_client(TmsdbGetData, 'tms_db_reader', callback_group=self.cb_group) while not self.cli_dbreader.wait_for_service(timeout_sec=1.0): self.get_logger().info('service "tms_db_reader" not available, waiting again...') req = TmsdbGetData.Request() req.tmsdb.id = id + 100000 # TODO: why add 100000 ? self.future_dbreader = self.cli_dbreader.call_async(req) await self.future_dbreader if self.future_dbreader.result() is not None: res = self.future_dbreader.result().tmsdb return res else: self.get_logger().info('Service "tms_db_reader" call failed %r' % (self.future_dbreader.exception(),)) async def read_name(self, id): tmsdb = await self.call_dbreader(int(id)) return tmsdb[0].name
def __init__(self): super().__init__('turtle_action_server') self.action_server = ActionServer(self, SquareSpiral, 'Spiral64', execute_callback= self.execute_callback, callback_group= ReentrantCallbackGroup(), goal_callback= self.goal_callback,)
def __init__(self): super().__init__('costmap_generator_mock') self._action_server = ActionServer(self, PlannerCostmap, 'generate_costmap', self.generate_costmap_callback)
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._goal_queue = collections.deque() self._goal_queue_lock = threading.Lock() self._current_goal = None self._action_server = ActionServer( self, Fibonacci, 'fibonacci', handle_accepted_callback=self.handle_accepted_callback, execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def handle_accepted_callback(self, goal_handle): """Starts or defers execution of an already accepted goal.""" with self._goal_queue_lock: if self._current_goal is not None: # Put incoming goal in the queue self._goal_queue.append(goal_handle) self.get_logger().info('Goal put in the queue') else: # Start goal execution right away self._current_goal = goal_handle self._current_goal.execute() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes a goal.""" try: self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) self.get_logger().info('Publishing feedback: {0}'.format( feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format( result.sequence)) return result finally: with self._goal_queue_lock: try: # Start execution of the next goal in the queue. self._current_goal = self._goal_queue.popleft() self.get_logger().info('Next goal pulled from the queue') self._current_goal.execute() except IndexError: # No goal in the queue. self._current_goal = None
def __init__(self) -> None: super().__init__('placer') self._place_server: ActionServer = ActionServer( self, PlaceObject, 'PlaceObject', self._place_cb)
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._goal_handle = None self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): """Provides a handle to an accepted goal.""" self.get_logger().info('Deferring execution...') self._goal_handle = goal_handle self._timer = self.create_timer(3.0, self.timer_callback) def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def timer_callback(self): # Execute the defered goal if self._goal_handle is not None: self._goal_handle.execute() self._timer.cancel() async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self.get_logger().info('Returning result: {0}'.format(result.sequence)) return result
class MoveDistanceActionServer(Node): def __init__(self): super().__init__('move_distance_action') self._goal_lock = threading.Lock() self._goal_handle = None self._action_server = ActionServer( self, MoveDistance, 'move_distance', execute_callback=self.execute_callback, handle_accepted_callback=self.handle_accepted_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accept or reject a client request to begin an action.""" self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: # This server only allows one goal at a time if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') # Abort the existing goal self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal_handle): """Accept or reject a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Execute a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = MoveDistance.Feedback() feedback_msg.progress.x = 0.0 for i in range(5): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return MoveDistance.Result() # Publish the feedback feedback_msg.progress.x = feedback_msg.progress.x + 0.1 goal_handle.publish_feedback(feedback_msg) # delay for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = MoveDistance.Result() result.complete = True self.get_logger().info('Goal finished') return result
class FSM(Node): def __init__(self): super().__init__('fsm') self.mode = "idle" self._goal_handle = None self._action_server = ActionServer( self, RunVirtualRail, 'run_virtual_rail', execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) #self.request_mode_service = self.create_service(self.request_mode) self.subscription = self.create_subscription( Joy, 'joy', self.manual_mode, 10) self.subscription def destroy(self): self._action_server.destroy() super().destroy_node() def request_mode(self,request, response): self.mode = request.mode response.success = True return response def goal_callback(self, goal_request='idle'): """Accepts or rejects a client request to begin an action.""" if self.mode == "virtual_rail": return GoalResponse.ACCEPT else: return GoalResponse.REJECT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): # this is where the actual virtual rail execution code goes # Start executing the action while True: start_time = current_time() # check if the goal has been cancelled if goal_handle.is_cancel_requested: goal_handle.canceled() result = Fibonacci.Result() result.success = False return Fibonacci.Result() if path_complete(): goal_handle.succeed() result = Fibonacci.Result() result.success = True return Fibonacci.Result() # Publish a pose to the position controller msg=Pose() pose.data = calc_position_setpoint(time, percent_complete) self.abs_pose_publisher.publish(msg) # Publish the feedback, could be current setpoint, percent complete, whatever we need to display in the GUI goal_handle.publish_feedback(feedback_msg) # Sleep for loop time while (current_time() - start_time < loop_time): ros.spin_once() # this is te asynchronous bit, allows other call backs to run sunch as cancelling the goal, rejecting incoming goals, basically Python asyncio def manual_mode(self, msg): x = msg.axes[1] y = msg.axes[0] a = math.atan2(x, y)+math.pi #v = math.sqrt(x**2 + y**2) w = msg.axes[3] v = -(((msg.axes[5]+1)/2)-1) drive_base_cmd = [a,v,w] self.get_logger().info('I heard: "%s"' % drive_base_cmd) def idle_mode(): pass # do nothing def check_vbus(): if vbus < threshold: self.state = error
class ActionServerWrapperProcess(multiprocessing.Process): def __init__(self, exit_future): multiprocessing.Process.__init__(self) self._node = rclpy.create_node('minimal_action_server') self._executor = MultiThreadedExecutor() self._exit_future = exit_future self._action_server = ActionServer( self._node, Fibonacci, 'fibonacci', execute_callback=self.execute_callback, callback_group=ReentrantCallbackGroup(), goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" # This server allows multiple goals in parallel self._node.get_logger().info('Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self._node.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Executes a goal.""" self._node.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self._node.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self._node.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes #time.sleep(0.5) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.sequence = feedback_msg.sequence self._node.get_logger().info('Returning result: {0}'.format(result.sequence)) return result def run(self): print("Server is entering...") while not self._exit_future.done(): rclpy.spin_until_future_complete( self._node, self._exit_future, executor=self._executor, timeout_sec=1) self._action_server.destroy() self._executor.shutdown() self._node.destroy_node() print('Server is exiting...')
def __init__(self): super().__init__('fibonacci_action_server') self._action_server = ActionServer(self, Fibonacci, 'fibonacci', self.execute_callback)
class GPIOActionServer(Node): def __init__(self): super().__init__('pi_gpio_server') pin_list = RaspberryPIGPIO.read_pins_from_file() self.pin_dic = {} for pin in pin_list: pin_id, type = pin.split(',') self.pin_dic[pin_id] = RaspberryPIGPIO(int(pin_id), type) self._goal_handle = None self._goal_lock = threading.Lock() #Node, action_type, action_name, execute_callback self._action_server = ActionServer( self, GPIO_Action, 'pi_gpio_server', execute_callback=self.execute_callback, goal_callback=self.goal_callback, handle_accepted_callback=self.handle_accepted_callback, cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup()) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def handle_accepted_callback(self, goal_handle): with self._goal_lock: if self._goal_handle is not None and self._goal_handle.is_active: self.get_logger().info('Aborting previous goal') self._goal_handle.abort() self._goal_handle = goal_handle goal_handle.execute() def cancel_callback(self, goal): self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT def execute_callback(self, goal_handle): """Executes the goal.""" self.get_logger().info('Executing goal...') # Populate goal message goal_msg = goal_handle.request.gpio # Populate feedback message feedback_msg = GPIO_Action.Feedback() feedback_msg.feedback = 1 # Populate result message result = GPIO_Action.Result() if not goal_handle.is_active: self.get_logger().info('Goal aborted') return GPIO_Action.Result() if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return GPIO_Action.Result() # Publish the feedback goal_handle.publish_feedback(feedback_msg) # get the pin ide and action type pin_id, action_type = goal_msg.split(',') if action_type == "high": self.pin_dic[pin_id].set_pin(1) time.sleep(0.1) result.value = 3 elif action_type == "low": self.pin_dic[pin_id].set_pin(0) time.sleep(0.1) result.value = 3 elif action_type == "read": result.value = GPIO.input(int(pin_id)) goal_handle.succeed() return result
class MinimalActionServer(Node): def __init__(self): super().__init__('minimal_action_server') self._sequence = [] self._action_server = ActionServer( self, Fibonacci, 'fibonacci', execute_callback=lambda x, self_=self: self_.execute_callback(x), callback_group=ReentrantCallbackGroup(), goal_callback=lambda x, self_=self: self_.goal_callback(x), cancel_callback=lambda x, self_=self: self_.cancel_callback(x)) def destroy(self): self._action_server.destroy() super().destroy_node() def goal_callback(self, goal_request): """Accepts or rejects a client request to begin an action.""" # This server allows multiple goals in parallel self.get_logger().info('Received goal request') return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): """Accepts or rejects a client request to cancel an action.""" self.get_logger().info('Received cancel request') return CancelResponse.ACCEPT async def execute_callback(self, goal_handle): """Executes a goal.""" self.get_logger().info('Executing goal...') # Append the seeds for the Fibonacci sequence feedback_msg = Fibonacci.Feedback() self._sequence = [0, 1] # Start executing the action for i in range(1, goal_handle.request.order): if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info('Goal canceled') return Fibonacci.Result() # Update Fibonacci sequence #feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1]) self._sequence.append(self._sequence[i] + self._sequence[i - 1]) #self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence)) self.get_logger().info('Publishing feedback: {0}'.format( self._sequence)) feedback_msg.value = self._sequence[-1] # Publish the feedback goal_handle.publish_feedback(feedback_msg) # Sleep for demonstration purposes time.sleep(1) goal_handle.succeed() # Populate result message result = Fibonacci.Result() result.result = self._sequence[-1] if len(self._sequence) > 0 else 1 #self.get_logger().info('Returning result: {0}'.format(result.sequence)) self.get_logger().info('Returning result: {0}'.format(result.result)) return result
def __init__(self, implementation='a*', map_in_python=True): super().__init__('maude_planner_action_server' ) # Node name, could be changed to fit the BT maude.init() maude.load(self.ASTAR_MAUDE_PATH[implementation]) self.astar_module = maude.getModule('ASTAR') if self.astar_module is None: self.get_logger().fatal( 'Cannot find Maude ASTAR module in {}'.format( self.ASTAR_MAUDE_PATH[implementation])) else: self.get_logger().info('Maude planner node is ready') self.occupancy_grid = None # It will be read and updated from the map topic self.maude_map = None self.amcl_pose = None # It will be read and updated from the topic /amcl_pose # Configures the action server to respond to ComputePathToPose actions self._action_server = ActionServer(self, ComputePathToPose, 'ComputePathToPose', self.action_callback) # Listen to map topic self._map_subscription = self.create_subscription( OccupancyGrid, 'global_costmap/costmap', self.map_callback, 10) # Queue size # Listen to topic /amcl_pose self._amcl_pose_subscription = self.create_subscription( PoseWithCovarianceStamped, 'amcl_pose', self.amcl_pose_callback, 10) # Queue size # Plan publisher (so that it is represented in the view) self._plan_publisher = self.create_publisher(Path, '/plan', 10) # Connect the Maude special operator to the map self.map_in_python = map_in_python if map_in_python: if implementation == 'a*': self.map_hook = MapProvider(self, self.astar_module) maude.connectEqHook('open2?', self.map_hook) else: self.map_hook = MapProviderGet(self) maude.connectEqHook('get', self.map_hook) # Find sorts and operators needed to construct the a* term # (once for all) m = self.astar_module pose_kind = m.findSort('Pose').kind() costmap_kind = m.findSort('CostMap').kind() float_kind = m.findSort('Float').kind() path_kind = m.findSort('Path').kind() int_kind = m.findSort('IntList').kind() # Init Goal NumRow NumCol # op a* : Pose Pose CostMap Float Float -> Path . self.astar_symb = m.findSymbol( self.ASTAR_OPNAME[implementation], [pose_kind, pose_kind, costmap_kind, float_kind, float_kind], path_kind) self.intlist_symb = m.findSymbol('_`,_', [int_kind] * 2, int_kind) self.cmap_symb = m.findSymbol('`{_`}', [int_kind], costmap_kind) # Constants that will be used multiple times self.zero_term = m.parseTerm('0') self.one_term = m.parseTerm('1') self.mtIL_term = m.parseTerm('mtIL')
def __init__(self): super().__init__('node2', namespace='name/space2') # Params #____________________________________________ # Publishers #____________________________________________ # pub1 # Qos profile qos_profile_pub1 = QoSPresetProfiles.SYSTEM_DEFAULT.value self.publisher_pub1 = self.create_publisher( M1, 'topic/path1', qos_profile=qos_profile_pub1) self.timer_pub1 = self.create_timer(10.0, self.publisher_call_pub1) self.i = 0 #_____ # Subscribers #____________________________________________ # Servers #____________________________________________ # Clients #____________________________________________ # c1 # Qos profile qos_profile_c1 = QoSProfile( history=QoSHistoryPolicy.KEEP_ALL, durability=QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability=QoSReliabilityPolicy.RELIABLE, depth=0) # Additional qos settings qos_profile_c1.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT qos_profile_c1.deadline.sec = 0 qos_profile_c1.deadline.nsec = 0 qos_profile_c1.lifespan.sec = 0 qos_profile_c1.lifespan.nsec = 0 qos_profile_c1.liveliness_lease_duration.sec = 0 qos_profile_c1.liveliness_lease_duration.nsec = 0 qos_profile_c1.avoid_ros_namespace_conventions = False self.client_c1 = self.create_client(Addtwo, 'service1', qos_profile=qos_profile_c1) #_____ # Action Servers #____________________________________________ # as2 self.action_server_as2 = ActionServer( self, Dec, 'as2', execute_callback=self.action_execute_call_as2, goal_callback=self.action_goal_call_as2, cancel_callback=self.action_cancel_call_as2) #_____ # Action Clients #____________________________________________ # ac1 self.action_client_ac1 = ActionClient(self, Increase, 'ac1')
class AnimationNode(Node): _feedback = PlayAnimation.Feedback _result = PlayAnimation.Result def __init__(self): """Starts a simple action server and waits for requests.""" super().__init__("animation") # currently, we set log level to info since the action server is spamming too much if not self.get_parameter("use_sim_time"): pass # todo how does this work in rclpy # rospy.on_shutdown(self.on_shutdown_hook) self.get_logger().debug("Starting Animation Server") self.current_joint_states = None self.hcm_state = 0 self.current_animation = None self.animation_cache = {} all_animations = find_all_animations_by_name(self) for animation_name, animation_file in all_animations.items(): try: with open(animation_file) as fp: self.animation_cache[animation_name] = parse(json.load(fp)) except IOError: self.get_logger().error("Animation '%s' could not be loaded" % animation_name) except ValueError: self.get_logger().error( "Animation '%s' had a ValueError. Probably there is a syntax error in the animation file. " "See traceback" % animation_name) traceback.print_exc() # predefined messages for performance self.anim_msg = AnimationMsg() # AnimationMsg takes a JointTrajectory message to also be able to process trajectories. To keep this # functionality, we use this message type, even though we only need a single joint goal in this case. self.traj_msg = JointTrajectory() self.traj_point = JointTrajectoryPoint() self.create_subscription(JointState, "joint_states", self.update_current_pose, 1) self.create_subscription(RobotControlState, "robot_state", self.update_hcm_state, 1) self.hcm_publisher = self.create_publisher(AnimationMsg, "animation", 1) self._as = ActionServer(self, PlayAnimation, "animation", self.execute_cb) def on_shutdown_hook(self): # we got external shutdown, let's still wait a bit, since we probably want to do a shutdown animation self.get_clock().sleep_for(Duration(seconds=5)) def execute_cb(self, goal): """ This is called, when someone calls the animation action""" first = True self.current_animation = goal.request.animation # publish info to the console for the user self.get_logger().info( f"Request to play animation {goal.request.animation}") if self.hcm_state != 0 and not goal.request.hcm: # 0 means controllable # we cant play an animation right now # but we send a request, so that we may can soon self.send_animation_request() self.get_logger().info( "HCM not controllable. Only sent request to make it come controllable." ) goal.abborted( text= "HCM not controllable. Will now become controllable. Try again later." ) return PlayAnimation.Result(successful=False) animator = self.get_animation_splines(self.current_animation, goal) while rclpy.ok() and animator: try: last_time = self.get_clock().now() # first check if we have another goal # todo this does not work in ros2 # self.check_for_new_goal(goal) # new_goal = self._as.current_goal.goal.animation ## if there is a new goal, calculate new splines and reset the time # if new_goal != self.current_animation: # self.current_animation = new_goal # animator = self.get_animation_splines(self.current_animation) # first = True # if we're here we want to play the next keyframe, cause there is no other goal # compute next pose t = float(self.get_clock().now().seconds_nanoseconds()[0] + self.get_clock().now().seconds_nanoseconds()[1] / 1e9) - animator.get_start_time() pose = animator.get_positions_rad(t) if pose is None: # see walking node reset # animation is finished # tell it to the hcm self.send_animation(False, True, goal.request.hcm, None, None) goal.publish_feedback( PlayAnimation.Feedback(percent_done=100)) # we give a positive result goal.succeed() return PlayAnimation.Result(successful=True) self.send_animation(first, False, goal.request.hcm, pose, animator.get_torque(t)) first = False # we have sent the first frame, all frames after this can't be the first perc_done = int( ((float(self.get_clock().now().seconds_nanoseconds()[0] + self.get_clock().now().seconds_nanoseconds()[1] / 1e9) - animator.get_start_time()) / animator.get_duration()) * 100) perc_done = max(0, min(perc_done, 100)) goal.publish_feedback( PlayAnimation.Feedback(percent_done=perc_done)) self.get_clock().sleep_until(last_time + Duration(seconds=0.02)) except (ExternalShutdownException, KeyboardInterrupt): exit() return PlayAnimation.Result(successful=False) def get_animation_splines(self, animation_name, goal): if animation_name not in self.animation_cache: self.get_logger().error("Animation '%s' not found" % animation_name) #goal.aborted(False, "Animation not found") return parsed_animation = self.animation_cache[animation_name] return SplineAnimator(parsed_animation, self.current_joint_states, self.get_logger(), self.get_clock()) def check_for_new_goal(self, goal): if self._as.is_new_goal_available(): next_goal = self._as.next_goal if not next_goal or not next_goal.get_goal(): return self.get_logger().debug("New goal: " + next_goal.get_goal().animation) if next_goal.get_goal().hcm: self.get_logger().debug("Accepted hcm animation %s", next_goal.get_goal().animation) # cancel old stuff and restart self._as.current_goal.set_aborted() self._as.accept_new_goal() else: # can't run this animation now self._as.next_goal.set_rejected() # delete the next goal to make sure, that we can accept something else self._as.next_goal = None self.get_logger().warn( "Couldn't start non hcm animation because another one is already running." ) def update_current_pose(self, msg): """Gets the current motor positions and updates the representing pose accordingly.""" self.current_joint_states = msg def update_hcm_state(self, msg): self.hcm_state = msg.state def send_animation_request(self): self.anim_msg.request = True self.anim_msg.header.stamp = self.get_clock().now().to_msg() self.hcm_publisher.publish(self.anim_msg) def send_animation(self, first, last, hcm, pose, torque): self.anim_msg.request = False self.anim_msg.first = first self.anim_msg.last = last self.anim_msg.hcm = hcm if pose is not None: self.traj_msg.joint_names = [] self.traj_msg.points = [JointTrajectoryPoint()] # We are only using a single point in the trajectory message, since we only want to send a single joint goal self.traj_msg.points[0].positions = [] self.traj_msg.points[0].effort = [] for joint in pose: self.traj_msg.joint_names.append(joint) self.traj_msg.points[0].positions.append(pose[joint]) if torque: # 1 and 2 should be mapped to 1 self.traj_msg.points[0].effort.append( np.clip((torque[joint]), 0, 1)) self.anim_msg.position = self.traj_msg self.anim_msg.header.stamp = self.get_clock().now().to_msg() self.hcm_publisher.publish(self.anim_msg)