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)
