ubuntu1804用ros在播放的kitti数据集上画出自车的形式轨迹
详细的过程去我的主页参考之前的博客，这里仅对代码的修改做个记录
一、作出汽车行驶过程中的全部轨迹
1、kitti.py
#!/usr/bin/env python
from data_utils import *
from publish_utils import *
from kitti_utils import *
import os
from kitti_utils import *
DATA_PATH = '/home/ros/dianyun/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync/'
def compute_3d_box_cam2(h,w,l,x,y,z,yaw):
"""
Return : 3xn in cam2 coordinate
"""
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0,1,0], [-np.sin(yaw),0 ,np.cos(yaw)]])
x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]
y_corners = [0,0,0,0,-h,-h,-h,-h]
z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2
class Object(): #guiji
def __init__(self):
self.locations = []
def update(self, displacement, yaw):
for i in range(len(self.locations)):
x0, y0 = self.locations[i]
x1 = x0*np.cos(yaw_change) +y0*np.sin(yaw_change) - displacement
y1 = -x0*np.sin(yaw_change) +y0*np.cos(yaw_change)
self.locations[i] = np.array([x1,y1])
self.locations += [np.array([0,0])]
def reset(self):
self.locations = []
if __name__ == "__main__":
frame = 0
rospy.init_node('kitti_node',anonymous=True)
cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
bridge = CvBridge()
ego_pub = rospy.Publisher('kitti_ego_car',Marker, queue_size=10)
# model_car_pub = rospy.Publisher('kitti_model_car',Marker, queue_size=10)
imu_pub = rospy.Publisher('kitti_imu',Imu, queue_size=10)
gps_pub = rospy.Publisher('kitti_gps',NavSatFix, queue_size=10)
box3d_pub = rospy.Publisher("kitti_3d", MarkerArray, queue_size=10)
loc_pub = rospy.Publisher("kitti_loc",MarkerArray, queue_size=10)
rate = rospy.Rate(10)
df_tracking = read_tracking("/home/ros/dianyun/2011_09_26_drive_0005_sync/data_tracking_label_2/training/label_02/0000.txt")
calib = Calibration("/home/ros/dianyun/2011_09_26_drive_0005_sync/2011_09_26/",from_video=True)
ego_car = Object()
prev_imu_data =None
while not rospy.is_shutdown():
df_tracking_frame = df_tracking[df_tracking.frame==frame]
boxes_2d = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])
types = np.array(df_tracking_frame['type'])
boxes_3d = np.array(df_tracking_frame[["height", "width", "length", "pos_x", "pos_y", "pos_z", "rot_y"]])
track_ids = np.array(df_tracking_frame['track_id'])
corners_3d_velos = []
for box_3d in boxes_3d:
corners_3d_cam2 = compute_3d_box_cam2(*box_3d)
corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)
corners_3d_velos += [corners_3d_velo]
image = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
point_cloud = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))
imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
if prev_imu_data is not None:
displacement = 0.1*np.linalg.norm(imu_data[["vf", "vl"]])
yaw_change = float(imu_data.yaw-prev_imu_data.yaw)
ego_car.update(displacement,yaw_change)
prev_imu_data = imu_data
publish_camera(cam_pub, bridge, image, boxes_2d, types)
publish_point_cloud(pcl_pub, point_cloud)
publish_3dbox(box3d_pub, corners_3d_velos,types, track_ids)
publish_ego_car(ego_pub)
# publish_car_model(model_car_pub)
publish_imu(imu_pub, imu_data )
publish_gps(gps_pub, imu_data ) #gps rviz cannot visulize, only use rostopic echo
publish_loc(loc_pub,ego_car.locations)
rospy.loginfo("kitti published")
rate.sleep()
frame += 1
if frame == 154:
frame = 0
ego_car.reset()
frame %= 1542、data_utils.py
#!/usr/bin/env python
import cv2
import numpy as np
import pandas as pd
IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf', 'vl', 'vu', 'ax', 'ay', 'az', 'af','al', 'au', 'wx', 'wy', 'wz', 'wf', 'wl', 'wu', 'posacc', 'velacc', 'navstat', 'numsats', 'posmode','velmode', 'orimode']
TRACKING_COLUMN_NAMES = ['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']
def read_camera(path):
return cv2.imread(path)
def read_point_cloud(path):
return np.fromfile(path,dtype=np.float32).reshape(-1, 4)
def read_imu(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = IMU_COLUMN_NAMES
return df
def read_tracking(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = TRACKING_COLUMN_NAMES
df = df[df['track_id']>=0] # remove DontCare objects
df.loc[df.type.isin(['Bus', 'Truck', 'Van', 'Tram']), 'type'] = 'Car' # Set all vehicle type to Car
df = df[df.type.isin(['Car', 'Pedestrian', 'Cyclist'])]
return df3、publish_utils.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from geometry_msgs.msg import Point
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import tf
import numpy as np
import cv2
FRAME_ID = 'map'
DETECTION_COLOR_DICT = {"Car":(255,255,0), "Pedestrian":(0,226,255), "Cyclist":(141,40,255)}
LIFETIME = 0.1
LINES = [[0,1],[1,2],[2,3],[3,0]]
LINES += [[4,5],[5,6],[6,7],[7,4]]
LINES += [[4,0],[5,1],[6,2],[7,3]]
LINES += [[4,1],[5,0]]
def publish_camera(cam_pub, bridge, image ,boxes ,types):
for typ, box in zip(types, boxes):
top_left = int(box[0]),int(box[1])
bottom_right = int(box[2]),int(box[3])
cv2.rectangle(image, top_left, bottom_right, DETECTION_COLOR_DICT[typ], 2)
cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))
def publish_point_cloud(pcl_pub,point_cloud):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = FRAME_ID
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids):
marker_array = MarkerArray()
for i ,corners_3d_velo in enumerate(corners_3d_velos):
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = i
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_LIST
b,g,r = DETECTION_COLOR_DICT[types[i]]
marker.color.r = r/255.0
marker.color.g = g/255.0
marker.color.b = b/255.0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.points = []
for l in LINES:
p1 = corners_3d_velo[l[0]]
marker.points.append(Point(p1[0],p1[1],p1[2]))
p2 = corners_3d_velo[l[1]]
marker.points.append(Point(p2[0],p2[1],p2[2]))
marker_array.markers.append(marker)
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = i+1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
p = np.mean(corners_3d_velo, axis=0)
text_marker.pose.position.x = p[0]
text_marker.pose.position.y = p[1]
text_marker.pose.position.z = p[2]+1
text_marker.text = str(track_ids[i])
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
b,g,r = DETECTION_COLOR_DICT[types[i]]
text_marker.color.r = r/255.0
text_marker.color.g = g/255.0
text_marker.color.b = b/255.0
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
box3d_pub.publish(marker_array)
def publish_ego_car(ego_car_pub):
# publish left and right 45 degree FOV lines and ego car model mesh
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
# line
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2 # line width
marker.points = []
# check the kitti axis model
marker.points.append(Point(5,-5,0)) # left up
marker.points.append(Point(0,0,0)) # center
marker.points.append(Point(5, 5,0)) # right up
ego_car_pub.publish(marker)
def publish_car_model(model_pub):
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()
mesh_marker.id = -1
mesh_marker.lifetime = rospy.Duration()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "/root/catkin_ws/src/kitti_tutorial/AudiR8.dae" #LOAD ERROR, DON'T KNOW WHY
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
mesh_marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(np.pi/2,0,np.pi)
mesh_marker.pose.orientation.x = q[0]
mesh_marker.pose.orientation.y = q[1]
mesh_marker.pose.orientation.z = q[2]
mesh_marker.pose.orientation.w = q[3]
mesh_marker.color.r = 1.0
mesh_marker.color.g = 1.0
mesh_marker.color.b = 1.0
mesh_marker.color.a = 1.0
mesh_marker.scale.x = 0.9
mesh_marker.scale.y = 0.9
mesh_marker.scale.z = 0.9
model_pub.publish(mesh_marker)
def publish_imu(imu_pub, imu_data, log=False):
"""
Publish IMU data
http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html
"""
imu = Imu()
imu.header.frame_id = FRAME_ID
imu.header.stamp = rospy.Time.now()
q = tf.transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), \
float(imu_data.yaw)) # prevent the data from being overwritten
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
imu.linear_acceleration.x = imu_data.af
imu.linear_acceleration.y = imu_data.al
imu.linear_acceleration.z = imu_data.au
imu.angular_velocity.x = imu_data.wf
imu.angular_velocity.y = imu_data.wl
imu.angular_velocity.z = imu_data.wu
imu_pub.publish(imu)
if log:
rospy.loginfo("imu msg published")
def publish_gps(gps_pub, gps_data, log=False):
"""
Publish GPS data
"""
gps = NavSatFix()
gps.header.frame_id = FRAME_ID
gps.header.stamp = rospy.Time.now()
gps.latitude = gps_data.lat
gps.longitude = gps_data.lon
gps.altitude = gps_data.alt
gps_pub.publish(gps)
if log:
rospy.loginfo("gps msg published")
def publish_loc(loc_pub, locations):
marker_array = MarkerArray()
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2
marker.points = []
for p in locations:
marker.points.append(Point(p[0],p[1],0))
marker_array.markers.append(marker)
loc_pub.publish(marker_array)二、保留最近20帧照片的轨迹
1、kitti.py
#!/usr/bin/env python
from data_utils import *
from publish_utils import *
from kitti_utils import *
import os
from kitti_utils import *
from collections import deque
DATA_PATH = '/home/ros/dianyun/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync/'
def compute_3d_box_cam2(h,w,l,x,y,z,yaw):
"""
Return : 3xn in cam2 coordinate
"""
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0,1,0], [-np.sin(yaw),0 ,np.cos(yaw)]])
x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]
y_corners = [0,0,0,0,-h,-h,-h,-h]
z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2
class Object(): #guiji
def __init__(self):
self.locations = deque(maxlen=20)#qu qian 20 zhen zhao pian de gui ji
def update(self, displacement, yaw):
for i in range(len(self.locations)):
x0, y0 = self.locations[i]
x1 = x0*np.cos(yaw_change) +y0*np.sin(yaw_change) - displacement
y1 = -x0*np.sin(yaw_change) +y0*np.cos(yaw_change)
self.locations[i] = np.array([x1,y1])
self.locations.appendleft(np.array([0,0]))
def reset(self):
self.locations = []
if __name__ == "__main__":
frame = 0
rospy.init_node('kitti_node',anonymous=True)
cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
bridge = CvBridge()
ego_pub = rospy.Publisher('kitti_ego_car',Marker, queue_size=10)
# model_car_pub = rospy.Publisher('kitti_model_car',Marker, queue_size=10)
imu_pub = rospy.Publisher('kitti_imu',Imu, queue_size=10)
gps_pub = rospy.Publisher('kitti_gps',NavSatFix, queue_size=10)
box3d_pub = rospy.Publisher("kitti_3d", MarkerArray, queue_size=10)
loc_pub = rospy.Publisher("kitti_loc",MarkerArray, queue_size=10)
rate = rospy.Rate(10)
df_tracking = read_tracking("/home/ros/dianyun/2011_09_26_drive_0005_sync/data_tracking_label_2/training/label_02/0000.txt")
calib = Calibration("/home/ros/dianyun/2011_09_26_drive_0005_sync/2011_09_26/",from_video=True)
ego_car = Object()
prev_imu_data =None
while not rospy.is_shutdown():
df_tracking_frame = df_tracking[df_tracking.frame==frame]
boxes_2d = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])
types = np.array(df_tracking_frame['type'])
boxes_3d = np.array(df_tracking_frame[["height", "width", "length", "pos_x", "pos_y", "pos_z", "rot_y"]])
track_ids = np.array(df_tracking_frame['track_id'])
corners_3d_velos = []
for box_3d in boxes_3d:
corners_3d_cam2 = compute_3d_box_cam2(*box_3d)
corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)
corners_3d_velos += [corners_3d_velo]
image = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
point_cloud = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))
imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
if prev_imu_data is not None:
displacement = 0.1*np.linalg.norm(imu_data[["vf", "vl"]])
yaw_change = float(imu_data.yaw-prev_imu_data.yaw)
ego_car.update(displacement,yaw_change)
prev_imu_data = imu_data
publish_camera(cam_pub, bridge, image, boxes_2d, types)
publish_point_cloud(pcl_pub, point_cloud)
publish_3dbox(box3d_pub, corners_3d_velos,types, track_ids)
publish_ego_car(ego_pub)
# publish_car_model(model_car_pub)
publish_imu(imu_pub, imu_data )
publish_gps(gps_pub, imu_data ) #gps rviz cannot visulize, only use rostopic echo
publish_loc(loc_pub,ego_car.locations)
rospy.loginfo("kitti published")
rate.sleep()
frame += 1
if frame == 154:
frame = 0
ego_car.reset()
frame %= 1542、data_utils.py
#!/usr/bin/env python
import cv2
import numpy as np
import pandas as pd
IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf', 'vl', 'vu', 'ax', 'ay', 'az', 'af','al', 'au', 'wx', 'wy', 'wz', 'wf', 'wl', 'wu', 'posacc', 'velacc', 'navstat', 'numsats', 'posmode','velmode', 'orimode']
TRACKING_COLUMN_NAMES = ['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']
def read_camera(path):
return cv2.imread(path)
def read_point_cloud(path):
return np.fromfile(path,dtype=np.float32).reshape(-1, 4)
def read_imu(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = IMU_COLUMN_NAMES
return df
def read_tracking(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = TRACKING_COLUMN_NAMES
df = df[df['track_id']>=0] # remove DontCare objects
df.loc[df.type.isin(['Bus', 'Truck', 'Van', 'Tram']), 'type'] = 'Car' # Set all vehicle type to Car
df = df[df.type.isin(['Car', 'Pedestrian', 'Cyclist'])]
return df3、publish_utils.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from geometry_msgs.msg import Point
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import tf
import numpy as np
import cv2
FRAME_ID = 'map'
DETECTION_COLOR_DICT = {"Car":(255,255,0), "Pedestrian":(0,226,255), "Cyclist":(141,40,255)}
LIFETIME = 0.1
LINES = [[0,1],[1,2],[2,3],[3,0]]
LINES += [[4,5],[5,6],[6,7],[7,4]]
LINES += [[4,0],[5,1],[6,2],[7,3]]
LINES += [[4,1],[5,0]]
def publish_camera(cam_pub, bridge, image ,boxes ,types):
for typ, box in zip(types, boxes):
top_left = int(box[0]),int(box[1])
bottom_right = int(box[2]),int(box[3])
cv2.rectangle(image, top_left, bottom_right, DETECTION_COLOR_DICT[typ], 2)
cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))
def publish_point_cloud(pcl_pub,point_cloud):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = FRAME_ID
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids):
marker_array = MarkerArray()
for i ,corners_3d_velo in enumerate(corners_3d_velos):
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = i
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_LIST
b,g,r = DETECTION_COLOR_DICT[types[i]]
marker.color.r = r/255.0
marker.color.g = g/255.0
marker.color.b = b/255.0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.points = []
for l in LINES:
p1 = corners_3d_velo[l[0]]
marker.points.append(Point(p1[0],p1[1],p1[2]))
p2 = corners_3d_velo[l[1]]
marker.points.append(Point(p2[0],p2[1],p2[2]))
marker_array.markers.append(marker)
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = i+1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
p = np.mean(corners_3d_velo, axis=0)
text_marker.pose.position.x = p[0]
text_marker.pose.position.y = p[1]
text_marker.pose.position.z = p[2]+1
text_marker.text = str(track_ids[i])
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
b,g,r = DETECTION_COLOR_DICT[types[i]]
text_marker.color.r = r/255.0
text_marker.color.g = g/255.0
text_marker.color.b = b/255.0
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
box3d_pub.publish(marker_array)
def publish_ego_car(ego_car_pub):
# publish left and right 45 degree FOV lines and ego car model mesh
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
# line
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2 # line width
marker.points = []
# check the kitti axis model
marker.points.append(Point(5,-5,0)) # left up
marker.points.append(Point(0,0,0)) # center
marker.points.append(Point(5, 5,0)) # right up
ego_car_pub.publish(marker)
def publish_car_model(model_pub):
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()
mesh_marker.id = -1
mesh_marker.lifetime = rospy.Duration()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "/root/catkin_ws/src/kitti_tutorial/AudiR8.dae" #LOAD ERROR, DON'T KNOW WHY
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
mesh_marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(np.pi/2,0,np.pi)
mesh_marker.pose.orientation.x = q[0]
mesh_marker.pose.orientation.y = q[1]
mesh_marker.pose.orientation.z = q[2]
mesh_marker.pose.orientation.w = q[3]
mesh_marker.color.r = 1.0
mesh_marker.color.g = 1.0
mesh_marker.color.b = 1.0
mesh_marker.color.a = 1.0
mesh_marker.scale.x = 0.9
mesh_marker.scale.y = 0.9
mesh_marker.scale.z = 0.9
model_pub.publish(mesh_marker)
def publish_imu(imu_pub, imu_data, log=False):
"""
Publish IMU data
http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html
"""
imu = Imu()
imu.header.frame_id = FRAME_ID
imu.header.stamp = rospy.Time.now()
q = tf.transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), \
float(imu_data.yaw)) # prevent the data from being overwritten
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
imu.linear_acceleration.x = imu_data.af
imu.linear_acceleration.y = imu_data.al
imu.linear_acceleration.z = imu_data.au
imu.angular_velocity.x = imu_data.wf
imu.angular_velocity.y = imu_data.wl
imu.angular_velocity.z = imu_data.wu
imu_pub.publish(imu)
if log:
rospy.loginfo("imu msg published")
def publish_gps(gps_pub, gps_data, log=False):
"""
Publish GPS data
"""
gps = NavSatFix()
gps.header.frame_id = FRAME_ID
gps.header.stamp = rospy.Time.now()
gps.latitude = gps_data.lat
gps.longitude = gps_data.lon
gps.altitude = gps_data.alt
gps_pub.publish(gps)
if log:
rospy.loginfo("gps msg published")
def publish_loc(loc_pub, locations):
marker_array = MarkerArray()
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2
marker.points = []
for p in locations:
marker.points.append(Point(p[0],p[1],0))
marker_array.markers.append(marker)
loc_pub.publish(marker_array)