@小鱼 在 realsense订阅无法进入sub_camera_callback 中说:
是不要反复订阅销毁,这样消耗的
谢谢鱼总回复。你那边有realsense可以帮忙试试吗?另外一个就是,其实我自己尝试的时候,也就手动一秒一次这样调用这个服务,这算是比较频繁的调用吗?可能实际使用中不会这样。
另外就是如果不做销毁的操作的话,好像就会主线程一直被rgbd两个话题的订阅操作所占用,没法进行之后的操作。
@小鱼 在 realsense订阅无法进入sub_camera_callback 中说:
是不要反复订阅销毁,这样消耗的
谢谢鱼总回复。你那边有realsense可以帮忙试试吗?另外一个就是,其实我自己尝试的时候,也就手动一秒一次这样调用这个服务,这算是比较频繁的调用吗?可能实际使用中不会这样。
另外就是如果不做销毁的操作的话,好像就会主线程一直被rgbd两个话题的订阅操作所占用,没法进行之后的操作。
命令行发这个就可以调用上面的服务
ros2 service call /camera_sub_add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}"
想要做的是,client端请求一个服务,然后在服务中先去订阅realsense的rgbd数据;之前存在的问题是,如果频繁的去调用这个订阅图像的函数,就会发生订阅不到的情况,然后至此开始,再怎么请求这个服务,都没法同时订阅rgb和d的数据。
然后我尝试先把这部分代码拆出来,结果发现现在上面的那段代码连订阅图像的那个call back函数都进不去了...可以确定的是,rgbd两个topic的名字是正确的。
#!/usr/bin/env python3
import time
import message_filters
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from example_interfaces.srv import AddTwoInts
class MinimalService(Node):
def __init__(self):
super().__init__('minimal_service_node')
self.srv = self.create_service(AddTwoInts, 'camera_sub_add_two_ints', self.add_two_ints_callback)
def add_two_ints_callback(self, request, response):
response.sum = request.a + request.b
flag, rgb, depth = self.sub_camera()
return response
def sub_camera(self):
rgb_subscriber = message_filters.Subscriber(self, Image, "/camera/color/image_raw")
depth_subscriber = message_filters.Subscriber(self, Image, "/camera/depth/image_rect_raw")
ts = message_filters.ApproximateTimeSynchronizer(
fs=[rgb_subscriber, depth_subscriber],
queue_size=20, slop=0.03)
vision_rgb_data, vision_depth_data = None, None
ts.registerCallback(self.sub_camera_callback, vision_rgb_data, vision_depth_data)
subscription_deadline = time.time() + 3.0
flag = True
while not all([vision_rgb_data, vision_depth_data]):
time.sleep(0.05)
if time.time() >= subscription_deadline:
print('Vision data subscription timeout.')
flag = False
break
del ts
self.destroy_subscription(rgb_subscriber.sub)
self.destroy_subscription(depth_subscriber.sub)
print('Vision data subscription finished.')
return flag, vision_rgb_data, vision_depth_data
def sub_camera_callback(self, rgb_msg, depth_msg, vision_rgb_data, vision_depth_data):
print("----------")
vision_rgb_data = rgb_msg
vision_depth_data = depth_msg
def main(args=None):
rclpy.init(args=args)
minimal_service = MinimalService()
rclpy.spin(minimal_service)
rclpy.shutdown()
if __name__ == '__main__':
main()
比如在一个node中创建了ABC三个服务,想要让他们能够并行执行的时候,我是给node加了MultiThreadedExecutor,然后所有的service都加了ReentrantCallbackGroup;这样确实可以用三个client同时向这三个服务发送请求,在终端中是可以看到node接收到了这三个请求;
但是如果想要实现:A和B两个服务是可以被两个client同时发送请求进行处理,但是当处理C服务请求的时候,A和B是不可以执行的。理想的处理方式是,在处理A和B请求的时候,如果收到C的请求,那么就等待A和B处理完之后再处理C,反之亦然。
我尝试过把C的callback_group变量去掉,我这里的服务的回调函数会去订阅相机的图片,这里就会提示订阅不到;如果把C的callback_group变量改为MutuallyExclusiveCallbackGroup,感觉并没有任何影响,还是可以同时请求ABC三个服务。
@小鱼 多谢鱼总,确实忽略了这个地方,大意了;不过对于上面的第二点和第四点还是比较困惑,鱼总之前有经验吗?
我想尝试以下几点:
(1)把这些变量、node都用ld.add_action()
的方式去实现,原来是return里面然后在[]里面一个一个加的,感觉用add的方式看起来更灵活一点,所以就想尝试一下上面的方法
(2)github代码中的思路应该是声明了一堆变量,然后把这些变量赋了默认值,在parameter里面,传入了这些变量,又传了一个配置文件(这两个地方有些变量是重复的,请问这个值会选哪个?)
(3)然后我就想把GitHub中使用的这个d435i.yaml
放到我自己ros pkg下的config目录下,但是把这个文件所在的路径传给parameter的时候,好像识别不了?
(4)最后一个问题是,可以把上面这一堆声明的变量和那个配置文件,写成一个配置文件吗?
参考官方 https://github.com/IntelRealSense/realsense-ros/blob/ros2/realsense2_camera/launch/rs_launch.py 的启动方式ros2 launch realsense2_camera rs_launch.py config_file:="'$COLCON_PREFIX_PATH/realsense2_camera/share/realsense2_camera/config/d435i.yaml'"
,是可以正常使用的。
然后我试着稍微修改了一些内容,发现了几个小问题,希望得到大家的帮助。下面是我自己调整了一下的代码:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import launch_ros.actions
configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'},
{'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'},
{'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'},
{'name': 'device_type', 'default': "''", 'description': 'choose device by type'},
{'name': 'config_file', 'default': "''", 'description': 'yaml config file'},
{'name': 'enable_pointcloud', 'default': 'false', 'description': 'enable pointcloud'},
{'name': 'unite_imu_method', 'default': "''", 'description': '[copy|linear_interpolation]'},
{'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'},
{'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
{'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'},
{'name': 'depth_width', 'default': '-1', 'description': 'depth image width'},
{'name': 'depth_height', 'default': '-1', 'description': 'depth image height'},
{'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'color_width', 'default': '-1', 'description': 'color image width'},
{'name': 'color_height', 'default': '-1', 'description': 'color image height'},
{'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'},
{'name': 'infra_width', 'default': '-1', 'description': 'infra width'},
{'name': 'infra_height', 'default': '-1', 'description': 'infra width'},
{'name': 'enable_infra1', 'default': 'true', 'description': 'enable infra1 stream'},
{'name': 'enable_infra2', 'default': 'true', 'description': 'enable infra2 stream'},
{'name': 'infra_rgb', 'default': 'false', 'description': 'enable infra2 stream'},
{'name': 'fisheye_width', 'default': '-1', 'description': 'fisheye width'},
{'name': 'fisheye_height', 'default': '-1', 'description': 'fisheye width'},
{'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'},
{'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'},
{'name': 'confidence_width', 'default': '-1', 'description': 'depth image width'},
{'name': 'confidence_height', 'default': '-1', 'description': 'depth image height'},
{'name': 'enable_confidence', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'fisheye_fps', 'default': '-1.', 'description': ''},
{'name': 'depth_fps', 'default': '-1.', 'description': ''},
{'name': 'confidence_fps', 'default': '-1.', 'description': ''},
{'name': 'infra_fps', 'default': '-1.', 'description': ''},
{'name': 'color_fps', 'default': '-1.', 'description': ''},
{'name': 'gyro_fps', 'default': '-1.', 'description': ''},
{'name': 'accel_fps', 'default': '-1.', 'description': ''},
{'name': 'color_qos', 'default': 'SYSTEM_DEFAULT', 'description': 'QoS profile name'},
{'name': 'confidence_qos', 'default': 'SYSTEM_DEFAULT', 'description': 'QoS profile name'},
{'name': 'depth_qos', 'default': 'SYSTEM_DEFAULT', 'description': 'QoS profile name'},
{'name': 'fisheye_qos', 'default': 'SYSTEM_DEFAULT', 'description': 'QoS profile name'},
{'name': 'infra_qos', 'default': 'SYSTEM_DEFAULT', 'description': 'QoS profile name'},
{'name': 'pointcloud_qos', 'default': 'SYSTEM_DEFAULT', 'description': 'QoS profile name'},
{'name': 'enable_gyro', 'default': 'false', 'description': ''},
{'name': 'enable_accel', 'default': 'false', 'description': ''},
{'name': 'pointcloud_texture_stream', 'default': 'RS2_STREAM_COLOR', 'description': 'testure stream for pointcloud'},
{'name': 'pointcloud_texture_index', 'default': '0', 'description': 'testure stream index for pointcloud'},
{'name': 'enable_sync', 'default': 'false', 'description': ''},
{'name': 'align_depth', 'default': 'false', 'description': ''},
{'name': 'filters', 'default': "''", 'description': ''},
{'name': 'clip_distance', 'default': '-2.', 'description': ''},
{'name': 'linear_accel_cov', 'default': '0.01', 'description': ''},
{'name': 'initial_reset', 'default': 'false', 'description': ''},
{'name': 'allow_no_texture_points', 'default': 'false', 'description': ''},
{'name': 'ordered_pc', 'default': 'false', 'description': ''},
{'name': 'calib_odom_file', 'default': "''", 'description': "''"},
{'name': 'topic_odom_in', 'default': "''", 'description': 'topic for T265 wheel odometry'},
{'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'},
{'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
{'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'},
{'name': 'temporal.holes_fill', 'default': '0', 'description': 'Persistency mode'},
{'name': 'stereo_module.exposure.1', 'default': '7500', 'description': 'Initial value for hdr_merge filter'},
{'name': 'stereo_module.gain.1', 'default': '16', 'description': 'Initial value for hdr_merge filter'},
{'name': 'stereo_module.exposure.2', 'default': '1', 'description': 'Initial value for hdr_merge filter'},
{'name': 'stereo_module.gain.2', 'default': '16', 'description': 'Initial value for hdr_merge filter'},
{'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
{'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
]
def declare_configurable_parameters(parameters):
return [
DeclareLaunchArgument(param['name'],
default_value=param['default'],
description=param['description'])
for param in parameters
]
def set_configurable_parameters(parameters):
return dict([(param['name'], LaunchConfiguration(param['name']))
for param in parameters])
def generate_launch_description():
"""Launch realsense2_camera node."""
node_configurable_parameters = declare_configurable_parameters(
configurable_parameters)
node_configurable_parameters_dict = set_configurable_parameters(
configurable_parameters)
config_file_path = os.path.join(
get_package_share_directory('robot_sensors'), 'config',
'handeye_vision_sensors_config.yaml')
config_file_path
handeye_vision_sensors_node = launch_ros.actions.Node(
package='realsense2_camera',
namespace=LaunchConfiguration('camera_name'),
name=LaunchConfiguration('camera_name'),
executable='realsense2_camera_node',
parameters=[node_configurable_parameters_dict],
output='screen',
emulate_tty=True,
)
ld = LaunchDescription()
# Declare the launch options.
for node_configurable_parameter in node_configurable_parameters:
ld.add_action(node_configurable_parameter)
ld.add_action(handeye_vision_sensors_node)
return ld