def __init__(self):
super().__init__('sonar2', namespace='')
# Params
#____________________________________________
# Publishers
#____________________________________________
# toCpu2
# Qos profile
qos_profile_toCpu2 = QoSProfile(
history=QoSHistoryPolicy.KEEP_LAST,
durability=QoSDurabilityPolicy.SYSTEM_DEFAULT,
reliability=QoSReliabilityPolicy.SYSTEM_DEFAULT,
depth=15)
# Additional qos settings
qos_profile_toCpu2.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT
qos_profile_toCpu2.deadline.sec = 0
qos_profile_toCpu2.deadline.nsec = 0
qos_profile_toCpu2.lifespan.sec = 0
qos_profile_toCpu2.lifespan.nsec = 0
qos_profile_toCpu2.liveliness_lease_duration.sec = 0
qos_profile_toCpu2.liveliness_lease_duration.nsec = 0
qos_profile_toCpu2.avoid_ros_namespace_conventions = False
self.publisher_toCpu2 = self.create_publisher(
Distance, 'topic/path/s2', qos_profile=qos_profile_toCpu2)
self.timer_toCpu2 = self.create_timer(0.0, self.publisher_call_toCpu2)
self.i = 0
def __init__(self):
super().__init__('simple_api_server')
api_req_qos_profile = QoSProfile(history=History.KEEP_LAST,
depth=1,
reliability=Reliability.RELIABLE,
durability=Durability.TRANSIENT_LOCAL)
self.task_api_req_pub = self.create_publisher(ApiRequest,
'/task_api_requests',
api_req_qos_profile)
self.get_building_map_srv = self.create_client(GetBuildingMap,
'/get_building_map')
# to show robot states
self.fleet_state_subscription = self.create_subscription(
FleetState,
'fleet_states',
self.fleet_state_cb,
qos_profile=QoSProfile(depth=20))
self.fleet_states_dict = {}
# TODO remove this
sim_time_bool = Parameter('use_sim_time', Parameter.Type.BOOL, True)
self.set_parameters([sim_time_bool])
self.task_states_cache = {}
def __init__(self):
super().__init__('simple_trajectory')
# Parameters
self.length_m = self.declare_parameter("length_m", 20.0).value
self.min_speed_mps = self.declare_parameter("min_speed_mps", 1.0).value
self.max_speed_mps = self.declare_parameter("max_speed_mps", 8.0).value
self.discretization_m = self.declare_parameter("discretization_m", 0.5).value
self.speed_increments = self.declare_parameter("speed_increments", 0.2).value
self.offset_distnace_m = self.declare_parameter("offset_distnace_m", 0.0).value
self.one_time = self.declare_parameter("one_time", False).value
self.timer_period_sec = self.declare_parameter("timer_period_sec", 0.0).value
if self.max_speed_mps < self.min_speed_mps:
self.max_speed_mps = self.min_speed_mps
# Publisher / Subscriber
qos = QoSProfile(depth=10)
if (self.one_time):
qos.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
qos.depth = 1
self.publisher_trajectory_ = self.create_publisher(
Trajectory, "trajectory", qos
)
self.subscriber_vehicle_kinematic_state_ = self.create_subscription(
VehicleKinematicState, "vehicle_kinematic_state",
self.vehicle_kinematic_state_cb, 0
)
# Local variables
self.trajectory_msg_ = None
self.vehicle_kinematic_state_msg_ = None
if (not self.one_time):
self.timer = self.create_timer(self.timer_period_sec, self.publish_trajectory)
def test_invalid_qos(self):
with self.assertRaises(InvalidQoSProfileException):
# No history or depth settings provided
QoSProfile()
with self.assertRaises(InvalidQoSProfileException):
# History is KEEP_LAST, but no depth is provided
QoSProfile(history=QoSHistoryPolicy.KEEP_LAST)
def __init__(self):
super().__init__('jarvis_camera')
self.declare_parameters(
namespace='',
parameters=[
('width', 640),
('height', 480),
('camera_topic', "camera_image"),
('time_period', 0.2),
]
)
timer_period = self.get_parameter("time_period").value
self.width = self.get_parameter("width").value
self.height = self.get_parameter("height").value
qos_profile = QoSProfile(depth=1)
qos_profile.history = QoSHistoryPolicy.KEEP_LAST
qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
self.pub = self.create_publisher(CompressedImage, self.get_parameter("camera_topic").value, qos_profile)
self.timer = self.create_timer(timer_period, self.timer_callback)
self.frame = None
self.has_frame = False
# self.bridge = CvBridge()
#cv2.VideoCapture(
# "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)1280, height=(int)720,format=(string)NV12, framerate=(fraction)24/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink")
self.get_logger().info("topic: %s, w: %d, h: %d, period: %.3f"%(self.get_parameter("camera_topic").value, self.width, self.height, timer_period))
def retrieve_urdf(self, timeout_sec: float = 5) -> None:
"""Retrieve the URDF file from the /robot_description topic.
Will raise an EnvironmentError if the topic is unavailable.
"""
self.logger.info('Retrieving URDF from "/robot_description"...')
qos_profile = QoSProfile(depth=1)
qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
self.urdf = None
def urdf_received(msg: String):
self.urdf = msg.data
self.create_subscription(
msg_type=String,
topic='/robot_description',
qos_profile=qos_profile,
callback=urdf_received,
)
rclpy.spin_once(self, timeout_sec=timeout_sec)
if self.urdf is None:
self.logger.error('Could not retrieve the URDF!')
raise EnvironmentError('Could not retrieve the URDF!')
def __init__(self, node):
self.node_ = node
self.topic_uvec_ = "/ifm3d/camera/unit_vectors"
self.topic_extr_ = "/ifm3d/camera/extrinsics"
self.topic_dist_ = "/ifm3d/camera/distance"
self.bridge_ = CvBridge()
self.lock_ = threading.Lock()
self.uvec_ = None
self.extr_ = None
self.sub_uvec_ = self.node_.create_subscription(
Image, self.topic_uvec_, self.uvec_cb,
QoSProfile(depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL))
qos = QoSProfile(depth=2,
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.VOLATILE)
self.sub_extr_ = self.node_.create_subscription(
Extrinsics, self.topic_extr_, self.extr_cb, qos)
self.sub_dist_ = self.node_.create_subscription(
Image, self.topic_dist_, self.dist_cb, qos)
def __init__(self):
super().__init__('rqt_ros2_knowledge_graph')
self.update_sub = self.create_subscription(
GraphUpdate,
'/graph_updates',
self.graph_update_callback,
qos_profile=QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=100,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE))
self.graph_pub = self.create_publisher(
GraphUpdate,
'/graph_updates',
qos_profile=QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=100,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE))
msg = GraphUpdate()
msg.operation_type = 3 # msg.REQSYNC
msg.element_type = 2 # msg.GRAPH
print(self.get_clock().now().seconds_nanoseconds())
msg.stamp.sec = self.get_clock().now().seconds_nanoseconds()[0]
msg.stamp.nanosec = self.get_clock().now().seconds_nanoseconds()[1]
msg.node_id = self.get_name()
self.graph_pub.publish(msg)
self.nodes = []
self.edges = []
def __init__(self):
super().__init__("repeater_py")
# You can play around with the QOS here, I could not find a significant improvement
# reliability = ReliabilityPolicy(ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
# history = HistoryPolicy(HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST)
# liveliness = LivelinessPolicy(LivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT)
# durability = DurabilityPolicy(DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)
# lifespan = Duration(seconds=0.5)
# deadline = Duration(seconds=0.5)
# depth = 1
# pub_qos = QoSProfile(
# reliability=reliability,
# depth=depth,
# history=history,
# liveliness=liveliness,
# durability=durability,
# lifespan=lifespan,
# deadline=deadline,
# )
# As the QOS doesn't really make any difference, lets keep the default one
pub_qos = QoSProfile(depth=10)
sub_qos = QoSProfile(depth=10)
self._pc_sub = self.create_subscription(PointCloud2, topic="pc_source", callback=self._handle_pc, qos_profile=sub_qos)
self._pc_pub = self.create_publisher(PointCloud2, topic="repeated_pc_py", qos_profile=pub_qos)
def __init__(self):
self.node = ros2.create_node("imu_sub")
qos_profile = QoSProfile(depth=1)
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
qos_profile.reliability = (
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
)
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
# subscribe to the accel topic, which will call
# __accel_callback every time the camera publishes data
self.__accel_sub = self.node.create_subscription(
Imu, self.__ACCEL_TOPIC, self.__accel_callback, qos_profile
)
# subscribe to the gyro topic, which will call
# __gyro_callback every time the camera publishes data
self.__gyro_sub = self.node.create_subscription(
Imu, self.__GYRO_TOPIC, self.__gyro_callback, qos_profile
)
self.__acceleration = np.array([0, 0, 0])
self.__acceleration_buffer = deque()
self.__angular_velocity = np.array([0, 0, 0])
self.__angular_velocity_buffer = deque()
def test_deprecation_warnings(self):
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always')
QoSProfile()
assert len(
w
) == 2 # must supply depth or history, _and_ KEEP_LAST needs depth
assert issubclass(w[0].category, UserWarning)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always')
# No deprecation if history is supplied
QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL)
assert len(w) == 0, str(w[-1].message)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always')
# Deprecation warning if KEEP_LAST, but no depth
QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST)
assert len(w) == 1
assert issubclass(w[0].category, UserWarning)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always')
# No deprecation if 'depth' is present
QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
assert len(w) == 0, str(w[-1].message)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always')
# No deprecation if only depth
QoSProfile(depth=1)
assert len(w) == 0, str(w[-1].message)
def __init__(self):
super().__init__("simple_publisher")
logger = self.get_logger()
self._num = 0
period = self.declare_parameter("period", 1.0)
depth = self.declare_parameter("qos_depth", 5)
logger.info("param period={}"
.format(period.get_parameter_value().double_value))
logger.info("param qos_depth={}"
.format(depth.get_parameter_value().integer_value))
profile = QoSProfile(depth=depth.get_parameter_value().integer_value)
profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
profile.liveliness = QoSLivelinessPolicy.AUTOMATIC
self._pub = self.create_publisher(SimpleMsg, "sample_topic", profile)
self.create_timer(
period.get_parameter_value().double_value,
self.on_timer_callback)
logger.info("publisher start!")
def __init__(self):
self.__bridge = CvBridge()
# ROS node
self.node = ros2.create_node("image_sub")
qos_profile = QoSProfile(depth=1)
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
qos_profile.reliability = (
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
)
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
# subscribe to the color image topic, which will call
# __color_callback every time the camera publishes data
self.__color_image_sub = self.node.create_subscription(
Image, self.__COLOR_TOPIC, self.__color_callback, qos_profile
)
self.__color_image = None
self.__color_image_new = None
# subscribe to the depth image topic, which will call
# __depth_callback every time the camera publishes data
self.__depth_image_sub = self.node.create_subscription(
Image, self.__DEPTH_TOPIC, self.__depth_callback, qos_profile
)
self.__depth_image = None
self.__depth_image_new = None
def test_default_profile(self):
with warnings.catch_warnings(record=True):
warnings.simplefilter('always')
profile = QoSProfile()
assert all(
profile.__getattribute__(k) ==
_qos_profile_default.__getattribute__(k)
for k in profile.__slots__)
def __init__(self):
super().__init__('base_info_handler')
self._world_link = "/map"
self._base_link = "/base_footprint"
self._last_cmd = TRVCommand()
try:
self.declare_parameter(self.URDF_PARAM)
except AttributeError:
pass # ROS 2 prior to dashing does not have this method, so ignore AttributeErrors
urdf_filename = self.get_parameter(self.URDF_PARAM).value
get_logger(self.get_name()).info("Robot URDF: %s" % urdf_filename)
self._odom = Odometry(
header=Header(frame_id=self._world_link),
child_frame_id=self._base_link,
pose=PoseWithCovariance(
pose=Pose(position=Point(x=0.0, y=0.0, z=0.0),
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))))
self._transform = TransformStamped(
header=Header(frame_id=self._world_link),
child_frame_id=self._base_link)
self.sub_robot_pose = self.create_subscription(
BaseInfo,
"/base_info",
self.base_info_cb,
qos_profile=qos_profile_sensor_data)
# these bridge from standard geometry_msgs/Twist to drive_base_msgs/TRVCommand
self.pub_cmd = self.create_publisher(TRVCommand,
"/drive_cmd",
qos_profile=QoSProfile(depth=1))
self.sub_joy = self.create_subscription(
Joy, "/joy", self.joy_cb, qos_profile=QoSProfile(depth=1))
# standard odom topic. every message replaces the last, so we only need one
self.pub_odom = self.create_publisher(Odometry,
"/odom",
qos_profile=QoSProfile(depth=1))
# tf topic. messages build a history, so give subscribers a chance to get them all
# CHECKME: use transient local?
self.pub_tf = self.create_publisher(TFMessage,
"/tf",
qos_profile=QoSProfile(depth=10))
# publish URDF "latched" -- doesn't seem to work, though
if urdf_filename is not None:
self.pub_urdf = self.create_publisher(
String,
"/robot_description",
qos_profile=QoSProfile(
depth=1,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL))
self._urdf_msg = String(data=urdf_filename)
self.pub_urdf.publish(self._urdf_msg)
self.timer_desc = self.create_timer(1, self.description_timer_cb)
def test_default_incompatible_qos_callbacks(self):
original_logger = rclpy.logging._root_logger
pub_log_msg = None
sub_log_msg = None
log_msgs_future = Future()
class MockLogger:
def get_child(self, name):
return self
def warn(self, message, once=False):
nonlocal pub_log_msg, sub_log_msg, log_msgs_future
if message.startswith('New subscription discovered'):
pub_log_msg = message
elif message.startswith('New publisher discovered'):
sub_log_msg = message
if pub_log_msg is not None and sub_log_msg is not None:
log_msgs_future.set_result(True)
rclpy.logging._root_logger = MockLogger()
qos_profile_publisher = QoSProfile(
depth=10,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)
self.node.create_publisher(EmptyMsg, self.topic_name,
qos_profile_publisher)
message_callback = Mock()
qos_profile_subscription = QoSProfile(
depth=10,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
self.node.create_subscription(EmptyMsg, self.topic_name,
message_callback,
qos_profile_subscription)
executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
rclpy.spin_until_future_complete(self.node, log_msgs_future, executor,
10.0)
if not self.is_fastrtps:
self.assertEqual(
pub_log_msg,
"New subscription discovered on topic '{}', requesting incompatible QoS. "
'No messages will be sent to it. '
'Last incompatible policy: DURABILITY_QOS_POLICY'.format(
self.topic_name))
self.assertEqual(
sub_log_msg,
"New publisher discovered on topic '{}', offering incompatible QoS. "
'No messages will be received from it. '
'Last incompatible policy: DURABILITY_QOS_POLICY'.format(
self.topic_name))
rclpy.logging._root_logger = original_logger
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('qos_subscription')
counter = Counter()
sub_default = node.create_subscription(
String,
'topic',
lambda msg: counter.onMsg('default', msg),
QoSProfile(depth=10))
sub_transient = node.create_subscription(
String,
'topic',
lambda msg: counter.onMsg('transient', msg),
QoSProfile(
depth=10,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL))
sub_volatile = node.create_subscription(
String,
'topic',
lambda msg: counter.onMsg('volatile', msg),
QoSProfile(
depth=10,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE))
sub_reliable = node.create_subscription(
String,
'topic',
lambda msg: counter.onMsg('reliable', msg),
QoSProfile(
depth=10,
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE))
sub_best_effort = node.create_subscription(
String,
'topic',
lambda msg: counter.onMsg('best_effort', msg),
QoSProfile(
depth=10,
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT))
# Asserts to suppress unused variable warnings.
assert sub_default
assert sub_transient
assert sub_volatile
assert sub_reliable
assert sub_best_effort
timer = node.create_timer(0.5, lambda: print(counter.count))
rclpy.spin(node)
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def __init__(self):
qos = QoSProfile(depth=10)
qos.durability = DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
super().__init__('minimal_subscriber')
self.subscription = self.create_subscription(String, 'topic',
self.listener_callback,
qos)
def __init__(self, robot_description):
super().__init__('robot_desc_publisher')
qos_profile = QoSProfile(depth=1)
qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
self.publisher_ = self.create_publisher(String, 'robot_description',
qos_profile)
self.robot_description = robot_description
self.timer_callback()
def __init__(self,
test_type: TestType,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ''):
super().__init__(node_name='nav2_tester', namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
transient_local_qos = QoSProfile(
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
volatile_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
transient_local_qos)
self.test_type = test_type
self.filter_test_result = True
if self.test_type == TestType.KEEPOUT:
self.plan_sub = self.create_subscription(Path, 'plan',
self.planCallback,
volatile_qos)
elif self.test_type == TestType.SPEED:
self.speed_it = 0
# Expected chain of speed limits
self.limits = [50.0, 0.0]
# Permissive array: all received speed limits must match to "limits" from above
self.limit_passed = [False, False]
self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit',
self.speedLimitCallback,
volatile_qos)
self.mask_received = False
self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask',
self.maskCallback,
transient_local_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
'navigate_to_pose')
def __init__(self,
buffer,
node,
*,
spin_thread=False,
qos=None,
static_qos=None):
"""
Constructor.
:param buffer: The buffer to propagate changes to when tf info updates.
:param node: The ROS2 node.
:param spin_thread: Whether to create a dedidcated thread to spin this node.
:param qos: A QoSProfile or a history depth to apply to subscribers.
:param static_qos: A QoSProfile or a history depth to apply to tf_static subscribers.
"""
if qos is None:
qos = QoSProfile(
depth=100,
durability=DurabilityPolicy.VOLATILE,
history=HistoryPolicy.KEEP_LAST,
)
if static_qos is None:
static_qos = QoSProfile(
depth=100,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
)
self.buffer = buffer
self.node = node
# Default callback group is mutually exclusive, which would prevent waiting for transforms
# from another callback in the same group.
self.group = ReentrantCallbackGroup()
self.tf_sub = node.create_subscription(TFMessage,
'tf',
self.callback,
qos,
callback_group=self.group)
self.tf_static_sub = node.create_subscription(
TFMessage,
'tf_static',
self.static_callback,
static_qos,
callback_group=self.group)
if spin_thread:
self.executor = SingleThreadedExecutor()
def run_func():
self.executor.add_node(self.node)
self.executor.spin()
self.executor.remove_node(self.node)
self.dedicated_listener_thread = Thread(target=run_func)
self.dedicated_listener_thread.start()
def __init__(self, node, device_key, wb_device, params=None):
super().__init__(node, device_key, wb_device, params)
self._camera_info_publisher = None
self._image_publisher = None
# Create topics
if not self._disable:
self._image_publisher = self._node.create_publisher(
Image, self._topic_name + '/image_raw',
QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
))
self._camera_info_publisher = self._node.create_publisher(
CameraInfo, self._topic_name + '/camera_info',
QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
))
# CameraInfo data
self.__message_info = CameraInfo()
self.__message_info.header.stamp = Time(
seconds=self._node.robot.getTime()).to_msg()
self.__message_info.height = self._wb_device.getHeight()
self.__message_info.width = self._wb_device.getWidth()
self.__message_info.distortion_model = 'plumb_bob'
focal_length = self._wb_device.getFocalLength()
if focal_length == 0:
focal_length = 570.34 # Identical to Orbbec Astra
self.__message_info.d = [0.0, 0.0, 0.0, 0.0, 0.0]
self.__message_info.r = [
1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0
]
self.__message_info.k = [
focal_length, 0.0,
self._wb_device.getWidth() / 2, 0.0, focal_length,
self._wb_device.getHeight() / 2, 0.0, 0.0, 1.0
]
self.__message_info.p = [
focal_length, 0.0,
self._wb_device.getWidth() / 2, 0.0, 0.0, focal_length,
self._wb_device.getHeight() / 2, 0.0, 0.0, 0.0, 1.0, 0.0
]
self._camera_info_publisher.publish(self.__message_info)
# Load parameters
camera_period_param = node.declare_parameter(
wb_device.getName() + '_period', self._timestep)
self._camera_period = camera_period_param.value
def __init__(self, node_name, queue_size=10, latch=False, rospy_init=True, **kwargs):
param = Parameter('use_sim_time', Parameter.Type.BOOL, True)
super(CompatibleNode, self).__init__(node_name, allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True, parameter_overrides=[param], **kwargs)
if latch:
self.qos_profile = QoSProfile(
depth=queue_size,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
else:
self.qos_profile = QoSProfile(depth=queue_size)
self.callback_group = None
def test_eq_operator(self):
profile_1 = QoSProfile()
profile_same = QoSProfile()
profile_different_number = QoSProfile(depth=1)
profile_different_duration = QoSProfile(deadline=Duration(seconds=2))
profile_equal_duration = QoSProfile(deadline=Duration(seconds=2))
self.assertEqual(profile_1, profile_same)
self.assertNotEqual(profile_1, profile_different_number)
self.assertNotEqual(profile_1, profile_different_duration)
self.assertEqual(profile_different_duration, profile_equal_duration)
def __init__(self):
super().__init__('nanosaur')
# Initialize eyes controller
self.eyes = eyes(self)
# Get rate joint_states
self.declare_parameter("rate", 5)
self.timer_period = 1. / float(self.get_parameter("rate").value)
# Get RPM motors
self.declare_parameter("rpm", 150)
self.rpm = int(self.get_parameter("rpm").value)
# Get parameter left wheel name
# https://index.ros.org/doc/ros2/Tutorials/Using-Parameters-In-A-Class-Python/
self.declare_parameter("motor.left.channel", 1)
left_id = int(self.get_parameter("motor.left.channel").value)
self.declare_parameter("motor.left.wheel", "sprocket_left_joint")
self.left_wheel_name = self.get_parameter("motor.left.wheel").value
# Get parameter right wheel name
self.declare_parameter("motor.right.channel", 4)
right_id = int(self.get_parameter("motor.right.channel").value)
self.declare_parameter("motor.right.wheel", "sprocket_right_joint")
self.right_wheel_name = self.get_parameter("motor.right.wheel").value
# Motor info message
self.get_logger().info(
f"RPM motors {self.rpm} - timer {self.timer_period}")
self.get_logger().info(
f"Motor left: Channel {left_id} - Wheel {self.left_wheel_name}")
self.get_logger().info(
f"Motor Right: Channel {right_id} - Wheel {self.right_wheel_name}")
# Load motors
self.mright = Motor(right_id, self.rpm)
self.mleft = Motor(left_id, self.rpm)
# Load subscriber robot_description
self.create_subscription(
String, 'robot_description',
lambda msg: self.configure_robot(msg.data),
QoSProfile(depth=1,
durability=rclpy.qos.QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL))
# joint state controller
# https://index.ros.org/doc/ros2/Tutorials/URDF/Using-URDF-with-Robot-State-Publisher/
self.joint_state = JointState()
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states',
qos_profile)
self.timer = self.create_timer(self.timer_period,
self.transform_callback)
# Drive control
self.p = [0.0, 0.0] # [right, left]
self.r = [0.0, 0.0] # [right, left]
self.subscription = self.create_subscription(Twist, 'cmd_vel',
self.drive_callback, 10)
self.subscription # prevent unused variable warning
# Node started
self.get_logger().info("Hello NanoSaur!")
def main():
rclpy.init()
node = rclpy.create_node('HDMAP_Server')
qos_profile = QoSProfile(depth=1)
qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
#hdmap_pub = node.create_publisher(HdmapMarker, 'hdmap_marker')
hdmapA_pub = node.create_publisher(HdmapArray, 'hdmap_array')
dgistMap = loadMap("dgist_a1.geojson").getItem()
dgistMap = dgistMap.fillna('nodata')
curb = dgistMap.loc[dgistMap['type'] == HdmapMarker.CURB]
whiteline = dgistMap.loc[dgistMap['type'] == HdmapMarker.WHITELINE]
dgistMap.loc[dgistMap['type'] == HdmapMarker.TRAFFICSIGN]
hdmap_array = HdmapArray()
hdmap_array.array = []
ros_time = node.get_clock().now().to_msg()
featuredMap = dgistMap.loc[dgistMap['type'] != 'Feature']
dataLen = len(featuredMap)
for idx in range(0, dataLen):
data = featuredMap.iloc[idx]
marker = HdmapMarker()
marker.id = idx
marker.scale = Vector3(x=0.2, y=0.2, z=0.2)
print(data)
marker.type = int(data['type'])
marker.header.stamp = ros_time
# TODO: support another specific type
try:
if marker.type == marker.TRAFFICSIGN:
coord_x, coord_y = data['geometry'].xy
marker.pose = Point(x=float(coord_x), y=float(coord_y))
else:
marker.points = []
coord_x, coord_y = data['geometry'].xy
for lat, lng in zip(coord_x, coord_x):
marker.points.append(Point(x=float(lat), y=float(lng)))
except Exception:
pass
hdmap_array.array.append(marker)
hdmap_array.header.stamp = ros_time
import time
while(1):
hdmapA_pub.publish(hdmap_array)
time.sleep(0.5)
def test_big_nanoseconds(self):
# Under 31 bits
no_problem = QoSProfile(lifespan=Duration(seconds=2))
self.convert_and_assert_equality(no_problem)
# Total nanoseconds in duration is too large to store in 32 bit signed int
int32_problem = QoSProfile(lifespan=Duration(seconds=4))
self.convert_and_assert_equality(int32_problem)
# Total nanoseconds in duration is too large to store in 32 bit unsigned int
uint32_problem = QoSProfile(lifespan=Duration(seconds=5))
self.convert_and_assert_equality(uint32_problem)
def _qos_profile(history, depth, reliability, durability):
# depth
profile = QoSProfile(depth=depth)
# history
if history.lower() == "keep_all":
profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL
elif history.lower() == "keep_last":
profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST
else:
raise RuntimeError("Invalid history policy: %s" % history)
# reliability
if reliability.lower() == "reliable":
profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
elif reliability.lower() == "best_effort":
profile.reliability = \
QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
else:
raise RuntimeError("Invalid reliability policy: %s" % reliability)
# durability
if durability.lower() == "transient_local":
profile.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
elif durability.lower() == "volatile":
profile.durability = \
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
else:
raise RuntimeError("Invalid durability policy: %s" % durability)
return profile
def run_topic_listening(node, topic_monitor, options):
"""Subscribe to relevant topics and manage the data received from susbcriptions."""
already_ignored_topics = set()
while rclpy.ok():
# Check if there is a new topic online
# TODO(dhood): use graph events rather than polling
topic_names_and_types = node.get_topic_names_and_types()
for topic_name, type_names in topic_names_and_types:
# Infer the appropriate QoS profile from the topic name
topic_info = topic_monitor.get_topic_info(topic_name)
if topic_info is None:
# The topic is not for being monitored
continue
if len(type_names) != 1:
if topic_name not in already_ignored_topics:
node.get_logger().info(
"Warning: ignoring topic '%s', which has more than one type: [%s]"
% (topic_name, ', '.join(type_names)))
already_ignored_topics.add(topic_name)
continue
type_name = type_names[0]
if not topic_monitor.is_supported_type(type_name):
if topic_name not in already_ignored_topics:
node.get_logger().info(
"Warning: ignoring topic '%s' because its message type (%s)"
'is not suported.' % (topic_name, type_name))
already_ignored_topics.add(topic_name)
continue
is_new_topic = topic_name and topic_name not in topic_monitor.monitored_topics
if is_new_topic:
# Register new topic with the monitor
qos_profile = QoSProfile(depth=10)
qos_profile.depth = QOS_DEPTH
if topic_info['reliability'] == 'best_effort':
qos_profile.reliability = \
QoSReliabilityPolicy.BEST_EFFORT
topic_monitor.add_monitored_topic(Header, topic_name, node,
qos_profile,
options.expected_period,
options.allowed_latency,
options.stale_time)
# Wait for messages with a timeout, otherwise this thread will block any other threads
# until a message is received
rclpy.spin_once(node, timeout_sec=0.05)
def __init__(self):
super().__init__('node3', namespace='name/space3')
# Params
#____________________________________________
# Publishers
#____________________________________________
# Subscribers
#____________________________________________
# sub1
# Qos profile
qos_profile_sub1 = QoSPresetProfiles.SYSTEM_DEFAULT.value
self.subscriber_sub1 = self.create_subscription(
M1,
'topic/path1',
self.subscriber_call_sub1,
qos_profile=qos_profile_sub1)
self.subscriber_sub1
#_____
# Servers
#____________________________________________
# Clients
#____________________________________________
# c2
# Qos profile
qos_profile_c2 = QoSProfile(
history=QoSHistoryPolicy.KEEP_ALL,
durability=QoSDurabilityPolicy.SYSTEM_DEFAULT,
reliability=QoSReliabilityPolicy.RELIABLE,
depth=0)
# Additional qos settings
qos_profile_c2.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT
qos_profile_c2.deadline.sec = 0
qos_profile_c2.deadline.nsec = 0
qos_profile_c2.lifespan.sec = 0
qos_profile_c2.lifespan.nsec = 0
qos_profile_c2.liveliness_lease_duration.sec = 0
qos_profile_c2.liveliness_lease_duration.nsec = 0
qos_profile_c2.avoid_ros_namespace_conventions = False
self.client_c2 = self.create_client(Addtwo,
'service2',
qos_profile=qos_profile_c2)
