@小鱼 改完了,有两处不同:
double seconds = this->now().seconds() ; //+ 1.0; #41行
rclcpp::WallRate loop_rate(20.0); //(1000.0); #63行
编译+source了。但是结果还是没有改变
@小鱼 改完了,有两处不同:
double seconds = this->now().seconds() ; //+ 1.0; #41行
rclcpp::WallRate loop_rate(20.0); //(1000.0); #63行
编译+source了。但是结果还是没有改变
@小鱼 我想起来教程里有个fishbot_bringup,我就直接拿来用了。但是虽然有了tf树,算法却还是跑不起起来。过程:
#MicroRos
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6
#laser
xhost + && sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY -p 8889:8888 registry.cn-hangzhou.aliyuncs.com/fishros/fishbot_laser
#tf
ros2 launch fishbot_bringup fishbot_bringup.launch.py
#carto
ros2 launch fishbot_cartographer cartographer.launch.py
#显示tf树
ros2 run rqt_tf_tree rqt_tf_tree
#rqt_graph
rqt_graph
我个人觉得是ros2新增的qos的问题。启动carto后慢慢翻终端里的信息会找到这么两条:
[cartographer_node-1] [WARN] [1692678150.099094890] [cartographer logger]: W0822 12:22:30.000000 11917 tf_bridge.cpp:53] Lookup would require extrapolation into the past. Requested time 1692678149.889606 but the earliest data is at time 1692678149.949214, when looking up transform from frame [odom] to frame [base_link]
[rviz2-3] [WARN] [1692678150.507302604] [rviz2]: New publisher discovered on topic '/scan', offering incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY
[ERROR] [cartographer_node-1]: process has died [pid 11917, exit code -6, cmd '/home/cxw/fishbot_ws/install/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/cxw/fishbot/install/fishbot_cartographer/share/fishbot_cartographer/config -configuration_basename fishbot_2d.lua --ros-args -r __node:=cartographer_node --params-file /tmp/launch_params_w6i5hcn_'].
第二条消息是说进程没了,感觉这应该是rqt_graph里连cartographer_node节点都刷不出来的原因?
小鱼大佬的docker雷达按2启动建图不是很顺利吗,能想到是什么原因吗?
@小鱼 找到了,贴一下过程:
sudo apt install ros-humble-rqt-tf-tree
rm ~/.config/ros.org/rqt_gui.ini #删除缓存,不然报错
ros2 run rqt_tf_tree rqt_tf_tree
但是结果还是一样的:
会和车的雷达要把fixed frame改成laser_frame有关系吗?默认好像是map
@小鱼 emm,是这样的:教程里是教我们下载carto源码,然后改配置文件.lua和launch文件,然后启动gazebo仿真,然后跑cartographer,这样做是没问题的,我的也没问题。
而我现在是把启动gazebo的仿真这一步换成了启动fishbot实车,启动了agent来获取scan,odom和imu数据。(在这同时把launch文件里的'use_sim_time'改成了’false‘,因为不是在跑仿真了)
然后我放一张rviz的图:
和一张rqt_graph的图:
我对其的理解是map没有数据,用ros2 topic echo去试也是没有数据跳出来的
不知道要如何才能像小鱼的雷达docker选2那样跑起来
@小鱼 我是想自己学写一写slam算法的。cartographer我已经把多少看了一遍了,所以我想了解一些cartographer实现方面的问题,比如上面这些。能教一教吗
@siviechen 补充一下,我的操作过程有(我没用gazebo仿真环境而是用实车):
启动microros的Agent:
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6
驱动雷达按1:
xhost + && sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY -p 8889:8888 registry.cn-hangzhou.aliyuncs.com/fishros/fishbot_laser
启动教程里的launch文件:
ros2 launch fishbot_cartographer cartographer.launch.py
@小鱼 嗯,我只是开启了docker里的雷达建图然后选2,仅此而已,然后提取了数据来做这个对比。这个选2有在建图啊,应该不是纯定位吧?
‘’这样一来你其实对比的是雷达定位和里程计定位的精度,而非地图精度‘’这句话我理解了。
另外想问一下,小鱼是怎么修改carto里的雷达订阅策略的?我按教程写的launch文件,然后跳出了雷达策略不对的日志:
[WARN] [1692271382.825165765] [ydlidar_node]: New subscription discovered on topic '/scan', requesting incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY
因为carto源码是ros1的,没有qos,我也没找出来怎么改(carto的代码也很有难度 )。佩服小鱼的docker里的选2是能跑起来的。能否指教一下?求求 有偿也可以,真的很想知道
教程链接:
https://fishros.com/d2lros2/#/humble/chapt10/get_started/3.%E9%85%8D%E7%BD%AEFishBot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE
@siviechen 再附带一个我当时建的图,这个算还可以吗?我感觉形状是对的。左边是入口和厕所门,中间是桌子的桌角和杂物。
怎么laser_frame会那么离谱呢?
最近看了tf树的使用,上网冲浪的时候也搜到了evo这种slam评价工具。因为我在fishbot跑建图时有看到tf的话题,我就想着用evo在fishbot上评价一下定位精度。
先说结果。我监听了tf里的laser_frame相对于map的位姿,并把他与小车发布的/odom话题进行比较,得到了相当离谱的图:
这里我把/odom话题的数据当作真值ground truth,tf树里laser_frame的位姿当作算法给的位姿。可以看到算法给的彩色轨迹相当的夸张(我跑了一圈我的宿舍,大致是长方形的,所以图里的ground truth是大致符合我印象里走的路线的)我想问是哪里出了问题呢?是不是不应该监听tf广播里的laser_frame?应该从哪里提取数据当作算法的位姿呢?
因为evo能跑起来给出结果,所以我感觉我写的提取数据的程序应该是没问题。以防万一在这里放一下过程:
使用了的命令行:
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6 #MicroRos
xhost + && sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY -p 8889:8888 registry.cn-hangzhou.aliyuncs.com/fishros/fishbot_laser #启动雷达按2
然后启动我自己的提取数据的节点,生成evo排版的文件:
提取/odom数据:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.clock import Clock
class NodeToEvo(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("transformation starts")
self.subscribe = self.create_subscription(Odometry,"odom",self.callback,rclpy.qos.qos_profile_sensor_data)
self.path = "/home/cxw"
self.file = self.path + "/ground_truth_odom" + '.txt'
self.writter = open(self.file,'w')
def callback(self,msg):
time_stamp = Clock().now()
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
self.writter.write(#str(msg.header.stamp.sec)+'.'+str(msg.header.stamp.nanosec)+' '+
str(time_stamp.to_msg().sec)+'.'+str(time_stamp.to_msg().nanosec)+' '+
str(position.x)+' '+
str(position.y)+' '+
str(position.z)+' '+
str(orientation.x)+' '+
str(orientation.y)+' '+
str(orientation.z)+' '+
str(orientation.w)+'\n')
self.get_logger().info("odom written successfully")
def main(args=None):
rclpy.init(args=args)
node = NodeToEvo("odom_to_evo")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
提取tf广播的laser_frame:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import tf_transformations # TF坐标变换库
from tf2_ros import TransformException # TF左边变换的异常类
from tf2_ros.buffer import Buffer # 存储坐标变换信息的缓冲类
from tf2_ros.transform_listener import TransformListener # 监听坐标变换的监听器类
from rclpy.clock import Clock
class tf(Node):
def __init__(self,name):
super().__init__(name)
self.tf_buffer=Buffer()
self.tf_lisenter = TransformListener(self.tf_buffer,self)
self.get_logger().info("tf_to_evo starts")
self.path = "/home/cxw"
self.file = self.path + "/tf_laser_frame" + '.txt'
self.writter = open(self.file,'w')
self.declare_parameter('source_frame', 'map') # 创建一个源坐标系名的参数
self.source_frame = self.get_parameter( # 优先使用外部设置的参数值,否则用默认值
'source_frame').get_parameter_value().string_value
self.declare_parameter('target_frame', 'laser_frame') # 创建一个目标坐标系名的参数
self.target_frame = self.get_parameter( # 优先使用外部设置的参数值,否则用默认值
'target_frame').get_parameter_value().string_value
self.timer = self.create_timer(0.05, self.on_timer) # 创建一个固定周期的定时器,处理坐标信息
def on_timer(self):
try:
time_stamp = Clock().now()
now = rclpy.time.Time() # 获取ROS系统的当前时间
trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换
self.target_frame,
self.source_frame,
now)
except TransformException as ex: # 如果坐标变换获取失败,进入异常报告
self.get_logger().info(
f'Could not transform {self.target_frame} to {self.source_frame}: {ex}')
return
position = trans.transform.translation # 获取位置信息
orientation = trans.transform.rotation # 获取姿态信息(四元数)
self.writter.write(str(time_stamp.to_msg().sec)+'.'+str(time_stamp.to_msg().nanosec)+' '+
str(position.x)+' '+
str(position.y)+' '+
str(position.z)+' '+
str(orientation.x)+' '+
str(orientation.y)+' '+
str(orientation.z)+' '+
str(orientation.w)+'\n')
self.get_logger().info("tf written successfully")
def main(args=None):
rclpy.init(args=args)
node = tf('tfevo')
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
最后调用evo画图:
evo_ape tum ground_truth_odom.txt tf_laser_frame.txt -va --plot --plot_mode xy
是不是不应该监听tf广播里的laser_frame?应该从哪里提取数据当作算法的位姿呢?
我想要自己写一个节点来对fishbot小车发布的odom数据进行evo文件排版的储存,但是终端提示qos策略不对,/odom话题是best_effort。以下是此种情况的代码和日志:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
class NodeToEvo(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("transformation starts")
self.subscribe = self.create_subscription(Odometry,"odom",self.callback,10)
self.path = "/home/cxw"
self.file = self.path + "/ground_truth_odom" + '.txt'
self.writter = open(self.file,'w')
def callback(self,msg):
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
self.writter.write(position.x+'\t'+position.y+'\t'+position.z+'\t'+orientation.x+'\t'+orientation.y+'\t'+orientation.z+'\t'+orientation.w+'\n')
def main(args=None):
rclpy.init(args=args)
node = NodeToEvo("ToEvo")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
然后启动microros接收小车的数据
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6
编译和source完后,ros2 run前面的py程序,日志报错如下:
cxw@pc:~/dev_ws$ ros2 run learning_topic toEvo
[INFO] [1691715773.163893894] [ToEvo]: transformation starts
[WARN] [1691715774.291494228] [ToEvo]: New publisher discovered on topic 'odom', offering incompatible QoS. No messages will be received from it. Last incompatible policy: RELIABILITY
然后查看小车odom的info:
cxw@pc:~/dev_ws$ ros2 topic info --verbose /odom
Type: nav_msgs/msg/Odometry
Publisher count: 1
Node name: fishbot_motion_control
Node namespace: /
Topic type: nav_msgs/msg/Odometry
Endpoint type: PUBLISHER
GID: 01.0f.bc.e6.2d.00.ca.28.01.00.00.00.00.00.01.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: BEST_EFFORT
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite
Subscription count: 0
然后我去网上查,查到了一些在程序里修改qos策略的贴子,但是还是没有跑起来。修改后的代码:
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
class NodeToEvo(Node):
def __init__(self,name):
super().__init__(name)
qos_profile = QoSProfile(
reliability = QoSReliabilityPolicy.BEST_EFFORT,
history = QoSHistoryPolicy.UNKNOWN,
depth = 10
)
self.get_logger().info("transformation starts")
self.subscribe = self.create_subscription(Odometry,"odom",self.callback,qos_profile)
self.path = "/home/cxw"
self.file = self.path + "/ground_truth_odom" + '.txt'
self.writter = open(self.file,'w')
def callback(self,msg):
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
self.writter.write(position.x+'\t'+position.y+'\t'+position.z+'\t'+orientation.x+'\t'+orientation.y+'\t'+orientation.z+'\t'+orientation.w+'\n')
def main(args=None):
rclpy.init(args=args)
node = NodeToEvo("ToEvo")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
编译后运行的终端log:
cxw@pc:~/dev_ws$ ros2 run learning_topic toEvo
[INFO] [1691716079.532903710] [ToEvo]: transformation starts
>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:
'Unknown QoS history policy, at ./src/qos.cpp:61'
with this new error message:
'create_publisher() failed setting topic QoS, at ./src/subscription.cpp:221'
rcutils_reset_error() should be called after error handling to avoid this.
<<<
>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:
'create_publisher() failed setting topic QoS, at ./src/subscription.cpp:221, at ./src/rcl/subscription.c:108'
with this new error message:
'invalid allocator, at ./src/rcl/subscription.c:218'
rcutils_reset_error() should be called after error handling to avoid this.
<<<
invalid allocator, at ./src/rcl/subscription.c:218
Traceback (most recent call last):
File "/home/cxw/dev_ws/install/learning_topic/lib/learning_topic/toEvo", line 33, in <module>
sys.exit(load_entry_point('learning-topic==0.0.0', 'console_scripts', 'toEvo')())
File "/home/cxw/dev_ws/install/learning_topic/lib/python3.10/site-packages/learning_topic/toEvo.py", line 34, in main
node = NodeToEvo("ToEvo")
File "/home/cxw/dev_ws/install/learning_topic/lib/python3.10/site-packages/learning_topic/toEvo.py", line 20, in __init__
self.subscribe = self.create_subscription(Odometry,"odom",self.callback,qos_profile)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/node.py", line 1370, in create_subscription
subscription_object = _rclpy.Subscription(
rclpy._rclpy_pybind11.RCLError: Failed to create subscription: invalid allocator, at ./src/rcl/subscription.c:218
[ros2run]: Process exited with failure 1
这下我就找不到解答这个的贴子了,求大佬指教
@小鱼 鱼佬!
但是我想理解一下整个过程。选项2里的源码实在docker里的,要在哪里看呢