You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
We are using a python script to overlay odometry feed from a cloud generated environment (.ply) that is converted into a 2d occupancy grid for autonomous navigation, it seems to be that there's a significant difference in latency between the odom feed in rtab_map and then using it in python. Additionally, when we do "reset odometry" in rtabmap, the odometry shown in rtabmap corrects itself once localised, but the odometry topic in python is not corrected, and becomes very incorrect. I was wondering if there was a specific reason or a severe oversight on our part. Thank you for your time.
Here is our code for extra context:
importosimportthreadingimportrospyimportopen3daso3dimportnumpyasnpimportmatplotlib.pyplotaspltfromsensor_msgs.msgimportPointCloud2fromnav_msgs.msgimportOdometryimportsensor_msgs.point_cloud2aspc2classOccupancyMapNode:
def_init_(self, resolution=0.01):
self.resolution=resolutionself.occupancy_grid=Noneself.odometry_x=0.0self.odometry_y=0.0self.odometry_yaw=0.0self.robot_height=Noneself.lock=threading.Lock()
self.height_threshold_lower=0.1# Exclude points below this height# Set the environment variableslocal_ip="172.17.0.2"remote_pi_ip="192.168.137.26"os.environ['ROS_IP'] =local_ipos.environ['ROS_MASTER_URI'] =f"http://{remote_pi_ip}:11311"rospy.init_node('occupancy_map_node', anonymous=True)
# Subscribe to the point cloud and odometry topicspoint_cloud_topic="/rtabmap/cloud_obstacles"odometry_topic="/rtabmap/odom"self.point_cloud_subscriber=rospy.Subscriber(point_cloud_topic, PointCloud2, self.point_cloud_callback)
self.odometry_subscriber=rospy.Subscriber(odometry_topic, Odometry, self.odometry_callback)
# Start ROS threadself.ros_thread=threading.Thread(target=self.ros_spin)
self.ros_thread.start()
plt.ion()
self.fig, self.ax=plt.subplots()
defros_spin(self):
rospy.spin()
defpoint_cloud_callback(self, msg):
points= []
forpinpc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True):
points.append([p[0], p[1], p[2]])
points=np.array(points)
iflen(points) ==0:
print("No points found in the point cloud :(")
returnwithself.lock:
self.occupancy_grid=pointsdefodometry_callback(self, msg):
position=msg.pose.pose.positionorientation=msg.pose.pose.orientationwithself.lock:
self.odometry_x=position.xself.odometry_y=position.yquaternion= (orientation.x, orientation.y, orientation.z, orientation.w)
euler=self.quaternion_to_euler(quaternion)
self.odometry_yaw=euler[2] # Yaw is the third elementdefquaternion_to_euler(self, quaternion):
# Convert quaternion to euler angles (roll, pitch, yaw)x, y, z, w=quaterniont0=+2.0* (w*x+y*z)
t1=+1.0-2.0* (x*x+y*y)
roll=np.arctan2(t0, t1)
t2=+2.0* (w*y-z*x)
t2=+1.0ift2>+1.0elset2t2=-1.0ift2<-1.0elset2pitch=np.arcsin(t2)
t3=+2.0* (w*z+x*y)
t4=+1.0-2.0* (y*y+z*z)
yaw=np.arctan2(t3, t4)
returnroll, pitch, yawdefdisplay_occupancy_map(self):
withself.lock:
ifself.occupancy_gridisNone:
return# Dynamically determine the ceiling height threshold based on the 90th percentileheight_threshold_upper=np.percentile(self.occupancy_grid[:, 2], 50)
# Filter points based on height thresholdsfiltered_points=self.occupancy_grid[(self.occupancy_grid[:, 2] >self.height_threshold_lower) &
(self.occupancy_grid[:, 2] <height_threshold_upper)]
self.ax.clear()
self.ax.scatter(filtered_points[:, 0], filtered_points[:, 1], s=1) # Display occupancy grid# Plot odometry position as a red dotself.ax.scatter(self.odometry_x, self.odometry_y, color='red')
self.ax.set_xlabel('X')
self.ax.set_ylabel('Y')
self.ax.set_title('Occupancy Map with Odometry Path')
plt.draw()
plt.gcf().canvas.flush_events()
node=OccupancyMapNode(resolution=0.01)
try:
whilenotrospy.is_shutdown():
node.display_occupancy_map()
plt.pause(0.1)
except (KeyboardInterrupt, SystemExit):
print("Shutting down")
rospy.signal_shutdown('Exiting...')
The text was updated successfully, but these errors were encountered:
For the odometry latency, do you mean there is more latency on python code or rtabmap's odometry?
For the odometry correctly, either you use TF to get map->odom transform to correct the received odometry, or subscribe to /rtabmap/mapGraph and read mapToOdom transform to correct the odometry afterwards.
Hi sir,
We are using a python script to overlay odometry feed from a cloud generated environment (.ply) that is converted into a 2d occupancy grid for autonomous navigation, it seems to be that there's a significant difference in latency between the odom feed in rtab_map and then using it in python. Additionally, when we do "reset odometry" in rtabmap, the odometry shown in rtabmap corrects itself once localised, but the odometry topic in python is not corrected, and becomes very incorrect. I was wondering if there was a specific reason or a severe oversight on our part. Thank you for your time.
Here is our code for extra context:
The text was updated successfully, but these errors were encountered: