def test_create_client_with_group(self):
cli1 = self.node.create_client(GetParameters, 'get/parameters')
group = ReentrantCallbackGroup()
cli2 = self.node.create_client(GetParameters, 'get/parameters', callback_group=group)
self.assertFalse(group.has_entity(cli1))
self.assertTrue(group.has_entity(cli2))
def main(args=None):
global g_node
rclpy.init(args=args)
g_node = rclpy.create_node('tms_ur_speaker')
g_node.get_logger().info('start')
subscription = g_node.create_subscription(String, 'speaker',
subscription_callback, 10)
subscription # prevent unused variable warning
srv = g_node.create_service(SpeakerSrv,
'speaker_srv',
service_callback,
callback_group=ReentrantCallbackGroup())
wav_srv = g_node.create_service(SpeakerWavSrv,
'speaker_wav_srv',
wav_srv_callback,
callback_group=ReentrantCallbackGroup())
while rclpy.ok():
rclpy.spin_once(g_node)
g_node.destroy_node()
rclpy.shutdown()
def test_create_timer_with_group(self):
tmr1 = self.node.create_timer(1.0, lambda: None)
group = ReentrantCallbackGroup()
tmr2 = self.node.create_timer(1.0, lambda: None, callback_group=group)
self.assertFalse(group.has_entity(tmr1))
self.assertTrue(group.has_entity(tmr2))
def test_create_service_with_group(self):
srv1 = self.node.create_service(GetParameters, 'get/parameters', lambda req: None)
group = ReentrantCallbackGroup()
srv2 = self.node.create_service(
GetParameters, 'get/parameters', lambda req: None, callback_group=group)
self.assertFalse(group.has_entity(srv1))
self.assertTrue(group.has_entity(srv2))
def test_create_subscription_with_group(self):
sub1 = self.node.create_subscription(Primitives, 'chatter', lambda msg: print(msg))
group = ReentrantCallbackGroup()
sub2 = self.node.create_subscription(
Primitives, 'chatter', lambda msg: print(msg), callback_group=group)
self.assertFalse(group.has_entity(sub1))
self.assertTrue(group.has_entity(sub2))
def test_reentrant_group(self):
self.assertIsNotNone(self.node.handle)
group = ReentrantCallbackGroup()
t1 = self.node.create_timer(1.0, lambda: None, callback_group=group)
t2 = self.node.create_timer(1.0, lambda: None, callback_group=group)
self.assertTrue(group.can_execute(t1))
self.assertTrue(group.can_execute(t2))
self.assertTrue(group.beginning_execution(t1))
self.assertTrue(group.beginning_execution(t2))
def test_multi_goal_accept(self):
executor = MultiThreadedExecutor(context=self.context)
action_server = ActionServer(
self.node,
Fibonacci,
'fibonacci',
execute_callback=self.execute_goal_callback,
callback_group=ReentrantCallbackGroup(),
handle_accepted_callback=lambda gh: None,
)
goal_msg = Fibonacci.Impl.SendGoalService.Request()
goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes))
future0 = self.mock_action_client.send_goal(goal_msg)
goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes))
future1 = self.mock_action_client.send_goal(goal_msg)
goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes))
future2 = self.mock_action_client.send_goal(goal_msg)
rclpy.spin_until_future_complete(self.node, future0, executor)
rclpy.spin_until_future_complete(self.node, future1, executor)
rclpy.spin_until_future_complete(self.node, future2, executor)
self.assertTrue(future0.result().accepted)
self.assertTrue(future1.result().accepted)
self.assertTrue(future2.result().accepted)
action_server.destroy()
def __init__(
self,
worker_id: int,
name: str,
policy_type: str = 'DQN') -> None:
"""Initialize Worker Node class."""
super().__init__(worker_id, name, policy_type)
self.episode = 0
# Set timer flags, futures, and callback groups.
self.flag = Flags(True, False, False, False, True)
self._future_weights = rclpy.Future()
self._future_gradients = rclpy.Future()
self._cb_group = ReentrantCallbackGroup()
self._total_reward = 0
self._update = 10
# Create ROS service clients and publisher.
self._cli: Dict[str, Client] = {
'weights': self.create_client(
Weights, '/weights', callback_group=self._cb_group),
'gradients': self.create_client(
Gradients, '/gradient', callback_group=self._cb_group),
}
self._pub = self.create_publisher(
Float32, 'total_reward', 10, callback_group=self._cb_group)
# Check if ROS service servers are available.
while not self._cli['weights'].wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'`Weights` service not available, waiting again...')
while not self._cli['gradients'].wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'`Gradients` service not available, waiting again...')
def __init__(self, task_tree, name_prefix=""):
self.node_type = task_tree[0]
self.task_tree = task_tree
if name_prefix != "":
name_prefix += "_"
#self.name = f"{name_prefix}{self.node_type}{TaskNode._task_num}"
self.name = f"{self.node_type}{TaskNode._task_num}"
TaskNode._task_num += 1
TaskNode.tasks.append(self)
self.cb_group = ReentrantCallbackGroup()
self.child_task_nodes = []
self.child_clients = [] # Action Client
self.goal_handles = [] # Goal Handle
super().__init__(self.name)
# self.get_logger().info(f"task tree:{self.task_tree}")
self.action_server = ActionServer(
self,
TsDoTask,
f"{self.name}",
self.execute_callback,
callback_group=self.cb_group,
# goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback)
self.get_logger().info('Instantiate')
def __init__(self):
super().__init__('turtlebot3_patrol_server')
"""************************************************************
** Initialise variables
************************************************************"""
self.goal = Patrol.Goal()
"""************************************************************
** Initialise ROS publishers and servers
************************************************************"""
qos = QoSProfile(depth=10)
# Initialise publishers
self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos)
# Initialise servers
self.action_server = ActionServer(
self,
Patrol,
'patrol',
execute_callback=self.execute_callback,
callback_group=ReentrantCallbackGroup(),
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback)
self.get_logger().info(
"Turtlebot3 patrol action server has been initialised.")
def __init__(self):
super().__init__('async_action_server')
self.logger = self.get_logger()
# fibonacciアクションサーバーの作成
# （execute_callback実行は複数同時処理を許可）
cg01 = ReentrantCallbackGroup()
# cg01 = MutuallyExclusiveCallbackGroup()
self._srv = ActionServer(self,
Fibonacci,
'fibonacci',
execute_callback=self._execute_callback,
callback_group=cg01)
# ---------- Timer 01 ----------
self._timer01_cnt = 0
self._timer01 = self.create_timer(timer_period_sec=3.0,
callback=self._on_timer01,
callback_group=cg01)
# ---------- Timer 01 ----------
self._timer02_cnt = 0
self._timer02 = self.create_timer(timer_period_sec=3.0,
callback=self._on_timer02,
callback_group=cg01)
self._dtstr = self._get_time_now()
self.logger.info('----- [action_server]Start! -----')
def __init__(self):
super().__init__("ex_follow_target_py")
# Create callback group that allows execution of callbacks in parallel without restrictions
self._callback_group = ReentrantCallbackGroup()
# Create MoveIt 2 interface
self._moveit2 = MoveIt2(
node=self,
joint_names=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.MOVE_GROUP_ARM,
execute_via_moveit=True,
callback_group=self._callback_group,
)
# Use upper joint velocity and acceleration limits
self._moveit2.max_velocity = 1.0
self._moveit2.max_acceleration = 1.0
# Create a subscriber for target pose
self.__previous_target_pose = Pose()
self.create_subscription(
msg_type=PoseStamped,
topic="/target_pose",
callback=self.target_pose_callback,
qos_profile=QoSProfile(depth=1),
callback_group=self._callback_group,
)
self.get_logger().info("Initialization successful.")
def __init__(self):
super().__init__("async_service_server")
self.logger = self.get_logger()
# add_two_intsサービスサーバーの作成
# （execute_callback実行は複数同時処理を許可）
cg_re_01 = ReentrantCallbackGroup()
cg_mu_01 = MutuallyExclusiveCallbackGroup()
self._srv = self.create_service(AddTwoInts,
"add_two_ints",
callback=self.add_two_ints_callback,
callback_group=cg_re_01
)
# ---------- Timer 01 ----------
self._timer01_cnt = 0
self._timer01 = self.create_timer(timer_period_sec=3.0,
callback=self._on_timer01,
callback_group=cg_mu_01)
# ---------- Timer 01 ----------
self._timer02_cnt = 0
self._timer02 = self.create_timer(timer_period_sec=3.0,
callback=self._on_timer02,
callback_group=cg_mu_01)
self._dtstr = self._get_time_now()
self.list_len = 3
self._share_list = list(range(self._timer01_cnt, self._timer01_cnt + self.list_len))
self.logger.info('----- [service_server]Start! -----')
async def subtask(self, goal_handle, task_tree):
# print("subtask")
command = task_tree[1]
subtask_client = ActionClient(self,
TsDoSubtask,
"subtask_node_" + str(command[0]),
callback_group=ReentrantCallbackGroup())
if not subtask_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_msg = TsDoSubtask.Goal()
if len(command) >= 2:
goal_msg.arg_json = command[1]
else:
goal_msg.arg_json = "{}"
print(f'subtask: subtask_node_{command[0]} args:{goal_msg.arg_json}')
subtask_goalhandle = await subtask_client.send_goal_async(goal_msg)
self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()] = subtask_goalhandle
if not subtask_goalhandle.accepted:
self.get_logger().info("goal rejected")
del self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()]
return "Abort"
future_result = await subtask_goalhandle.get_result_async()
del self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()]
self.get_logger().info(f"returned {future_result.result.message}")
return future_result.result.message
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
def __init__(self):
super().__init__('wait')
self.cb_group = ReentrantCallbackGroup()
self.srv = self.create_service(AddTwoInts,
'wait',
self.wait_callback,
callback_group=self.cb_group)
async def create_tasknode(self, task_tree):
global executor
# generate task_node
child_tasknode = TaskNode(task_tree)
# self.child_tasknode.append(self.childtask_node)
executor.add_node(child_tasknode) # added last added task
## execute
client = ActionClient(self,
TsDoTask,
child_tasknode.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 = TsDoTask.Result()
result.message = "Abort"
return result
goal = TsDoTask.Goal()
goal_handle_client = await client.send_goal_async(goal)
if not goal_handle_client.accepted:
self.get_logger().info("goal reject")
goal_handle.abort()
result = TsDoTask.Result()
result.message = "Goal Reject"
return result
self.get_logger().info("goal accept")
return goal_handle_client, child_tasknode
def __init__(self):
super().__init__('camera_receiver')
self.pubImg = self.create_publisher(Image, "camera/image", 10)
self.pubImgCompress = self.create_publisher(CompressedImage,
"camera/compressed", 10)
self.declare_parameter("ip", "192.168.2.101")
self.declare_parameter("frame_id", "cellphone_camera")
ip = self.get_parameter("ip").get_parameter_value().string_value
self.cap = cv.VideoCapture("http://" + ip + ":8080/video")
self.cap.set(cv.CAP_PROP_BUFFERSIZE, 0)
self.callbackGroup = ReentrantCallbackGroup()
timer_period = 0.016666 / 2.0 # segundos
self.timer = self.create_timer(
timer_period,
self.timerCallback) #, callback_group=self.callbackGroup)
self.timer2 = self.create_timer(
timer_period,
self.timer2Callback) #, callback_group=self.callbackGroup)
self.frame = None
self.ret = False
self.height = 0
self.width = 0
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__("ex_throw_object")
# Create callback group that allows execution of callbacks in parallel without restrictions
self._callback_group = ReentrantCallbackGroup()
# Create MoveIt 2 interface
self._moveit2 = MoveIt2(
node=self,
joint_names=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.MOVE_GROUP_ARM,
callback_group=self._callback_group,
)
# Use upper joint velocity and acceleration limits
self._moveit2.max_velocity = 1.0
self._moveit2.max_acceleration = 1.0
# Create MoveIt 2 interface for gripper
self._moveit2_gripper = MoveIt2Gripper(
node=self,
gripper_joint_names=panda.gripper_joint_names(),
open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS,
closed_gripper_joint_positions=panda.
CLOSED_GRIPPER_JOINT_POSITIONS,
gripper_group_name=panda.MOVE_GROUP_GRIPPER,
callback_group=self._callback_group,
)
self.get_logger().info("Initialization successful.")
def __init__(self) -> None:
super().__init__('battery_state_server')
# tworzenie zmiennych potrzebnych do obliczania SOC i time_to_full
self.goal_handle = None
self.measure_time = None
self.start = None
self.last_callback_time = None
self.state_of_charge = 0 # A*s
self.max_capacity = 5 * 60 * 60 # A*s
self.measurements = []
# listy potrzebne do logowania danych
self.log_time, self.log_soc, self.log_current = [], [], []
# tworzymy callback grupę żeby możliwe było równoczesne przetwarzanie callbacków
callback_group = ReentrantCallbackGroup()
# tworzymy serwer akcji obliczający stan baterii
self._action_server = ActionServer(self,
BatteryState,
'battery_state',
self.execute_callback,
callback_group=callback_group)
# tworzymy subscribera do tematu z obciążeniem prądowym
self.subscription = self.create_subscription(
Float64,
'current',
self.analysis_callback,
10,
callback_group=callback_group)
self.get_logger().info('Starting battery state server')
def camera_listener(self):
"""Registers the node to listen to CameraMsg(video_mjpeg) and
Image(display_mjpeg) messages.
"""
camera_sub_cb_grp = ReentrantCallbackGroup()
self.create_subscription(CameraMsg,
constants.CAMERA_MSG_TOPIC,
self.camera_callback,
10,
callback_group=camera_sub_cb_grp)
display_img_sub_cb_grp = ReentrantCallbackGroup()
self.create_subscription(Image,
constants.DISPLAY_MSG_TOPIC,
self.display_callback,
10,
callback_group=display_img_sub_cb_grp)
def __init__(self, namespace: str = None):
super().__init__('each_leg_gait', namespace=namespace)
self._gait_action_server = ActionServer(
self,
Gait,
'gait',
execute_callback=self.execute_callback,
callback_group=ReentrantCallbackGroup(),
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback)
self._calibrate_signal_pub = self.create_publisher(
Empty, 'calibrate_signal', 10)
self._move_leg_action_client = ActionClient(self, MoveLeg, 'move_leg')
self._forcesensor_sub = self.create_subscription(
ForceSensor, 'force_sensor', self.forcesensor_callback, 10)
# self._timer = self.create_timer(0.001, lambda: self.get_logger().info('timer'))
self._leg_stop_condition = (lambda: False)
self._leg_stoped = False
self._move_leg_goal_handle = None
self._forcesensor_msg = None
self._x_result = 0.0
self._y_result = 0.0
self._z_result = 0.0
self._z_lock = 0.0
self._x_offset = 0
self._z_offset = 0.0
self._timer_start = time.time()
def __init__(self, role_name, ego_vehicle_id, node, avoid_risk=True):
"""
"""
super(BasicAgent, self).__init__(role_name, ego_vehicle_id, avoid_risk,
node)
self.node = node
self._avoid_risk = avoid_risk
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
if ROS_VERSION == 1:
cb_group = None
elif ROS_VERSION == 2:
cb_group = ReentrantCallbackGroup()
if self._avoid_risk:
self._vehicle_id_list = []
self._lights_id_list = []
self._objects = []
self._actors_subscriber = self.node.create_subscriber(
CarlaActorList,
"/carla/actor_list",
self.actors_updated,
callback_group=cb_group)
self._objects_subscriber = self.node.create_subscriber(
ObjectArray, "/carla/{}/objects".format(role_name),
self.objects_updated)
self._get_actor_waypoint_client = self.node.create_service_client(
'/carla_waypoint_publisher/{}/get_actor_waypoint'.format(
role_name),
GetActorWaypoint,
callback_group=cb_group)
def create_renv_client(self, service_type, service_name):
cb_group = ReentrantCallbackGroup()
service_type_for_ros2 = service_type
result_sub = self.create_renv_subscription(service_type.Response,
service_name + '_response',
10,
without_ros_subscription=True,
event_prefix='rosservice')
def request_callback(request_msg, self_=self, service_name=service_name):
cli = self_.__clients[service_name]
future = cli.call_async(request_msg)
future.add_done_callback(lambda future: result_sub(future.result()))
#rclpy.spin_until_future_complete(self_, future)
#result = future.result()
#result_sub(result)
pass
self.create_renv_publisher(service_type.Request,
service_name + '_request',
10,
callback=request_callback,
without_ros_publisher=True,
action_prefix='rosservice')
cli = self.create_client(service_type_for_ros2, service_name, callback_group=cb_group)
self.__clients[service_name] = cli
return cli
async def play_jtalk(self, t):
self.cli_speaker = self.create_client(
SpeakerSrv, 'speaker_srv', callback_group=ReentrantCallbackGroup())
while not self.cli_speaker.wait_for_service(1.0):
self.get_logger().info('service "speaker_srv" not found...')
req = SpeakerSrv.Request()
req.data = t
await self.cli_speaker.call_async(req)
def __init__(self):
super().__init__('hello_action_test_server')
self.hello_server = ActionServer(self, Hello, 'hello_action',
execute_callback=self.executeCB,
callback_group=ReentrantCallbackGroup(),
goal_callback=self.goalCB,
cancel_callback=self.cancelCB)
def __init__(self):
super().__init__('minimal_action_server')
self._action_server = ActionServer(
self,
Fibonacci,
'fibonacci',
execute_callback=self.execute_callback,
callback_group=ReentrantCallbackGroup())
def __init__(self, node):
super().__init__(ReentrantCallbackGroup())
self.guard_condition = _rclpy.rclpy_create_guard_condition(node.context.handle)
self.guard_condition_index = None
self.guard_is_ready = False
self.node = node
self.future = None
def __init__(self):
super().__init__()
self.log_subscriber = self.create_subscription(
Log,
"/rosout",
self.log_callback,
callback_group=ReentrantCallbackGroup())
self.is_navigation_end = False
self.is_navigation_start = False
