carla ros bridge自车轨迹录制

最新推荐文章于 2024-09-13 08:27:34 发布

sea_lichenglin

最新推荐文章于 2024-09-13 08:27:34 发布

阅读量585

点赞数 1

分类专栏： carla 文章标签： python 人工智能

本文链接：https://blog.csdn.net/Fakerlinux/article/details/128120501

版权

carla 专栏收录该内容

6 篇文章 2 订阅

订阅专栏

import ros_compatibility as roscomp
from ros_compatibility.node import CompatibleNode
import csv
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import NavSatFix


# uint8 COVARIANCE_TYPE_UNKNOWN=0
# uint8 COVARIANCE_TYPE_APPROXIMATED=1
# uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
# uint8 COVARIANCE_TYPE_KNOWN=3
# std_msgs/Header header
#   uint32 seq
#   time stamp
#   string frame_id
# sensor_msgs/NavSatStatus status
#   int8 STATUS_NO_FIX=-1
#   int8 STATUS_FIX=0
#   int8 STATUS_SBAS_FIX=1
#   int8 STATUS_GBAS_FIX=2
#   uint16 SERVICE_GPS=1
#   uint16 SERVICE_GLONASS=2
#   uint16 SERVICE_COMPASS=4
#   uint16 SERVICE_GALILEO=8
#   int8 status
#   uint16 service
# float64 latitude
# float64 longitude
# float64 altitude
# float64[9] position_covariance
# uint8 position_covariance_type

# nancyhu@nancyhu-pc:~/Desktop/lcl_prj/carla/carla_ros_bridge/catkin_ws$ rosmsg show sensor_msgs/NavSatFix




ROS_VERSION = roscomp.get_ros_version()

class record_globalroute(CompatibleNode):
    def __init__(self):
        self.csv_data= open("./src/ros-bridge/ros-bridge-master/carla_globalroute/src/global_route/test.csv", "w")
        self.csv_header=["header.stamp","header.frame_id","poses.header.stamp","poses.header.frame_id",
        "poses.pose.position.x","poses.pose.position.y","poses.pose.position.z","poses.pose.orientation.x",
        "poses.pose.orientation.y","poses.pose.orientation.z","poses.pose.orientation.w","latitude","longitude","altitude",
        "position_covariance"]
        self.writer_data = csv.DictWriter(self.csv_data, self.csv_header)
        self.writer_data.writeheader()
    def run(self):
        self.callback_group = roscomp.callback_groups.ReentrantCallbackGroup()
        # self.global_route_subscriber = self.new_subscription(
        #         Odometry,
        #         "/carla/ego_vehicle/odometry",
        #         self.record_data,
        #         qos_profile=10, callback_group=self.callback_group)
        self.global_gnss_subscriber = self.new_subscription(
                NavSatFix,
                "/carla/ego_vehicle/gnss",
                self.record_data,
                qos_profile=10, callback_group=self.callback_group)
    def record_data(self,data):
        #ego
        # self.writer_data.writerow({
        #  "header.stamp": " ",
        #  "header.frame_id": " ",
        #  "poses.header.stamp": " ",
        #  "poses.header.frame_id": " ",
        #  "poses.pose.position.x": data.pose.pose.position.x,
        #  "poses.pose.position.y": data.pose.pose.position.y,
        #  "poses.pose.position.z": data.pose.pose.position.z,
        #  "poses.pose.orientation.x": data.pose.pose.orientation.x,
        #  "poses.pose.orientation.y": data.pose.pose.orientation.y,
        #  "poses.pose.orientation.z": data.pose.pose.orientation.z,
        #  "poses.pose.orientation.w": data.pose.pose.orientation.w,
        #  "latitude": " ",
        #  "longitude": " "
        #  })
         #gnss
        self.writer_data.writerow({
        "header.stamp": " ",
        "header.frame_id": " ",
        "poses.header.stamp": " ",
        "poses.header.frame_id": " ",
        "poses.pose.position.x": " ",
        "poses.pose.position.y": " ",
        "poses.pose.position.z": " ",
        "poses.pose.orientation.x": " ",
        "poses.pose.orientation.y": " ",
        "poses.pose.orientation.z": " ",
        "poses.pose.orientation.w": " ",
        "latitude": data.latitude,
        "longitude": data.longitude
        })

    def close(self):
        self.csv_data.close()
        self.destroy_subscription( self.global_route_subscriber)
        
def main(args=None):
    roscomp.init("carla_record_globalroute", args=args)
    record_globalroute_tmp = record_globalroute()
    try:
        record_globalroute_tmp.run()
        record_globalroute_tmp.spin()
    except KeyboardInterrupt:
        pass
    finally:
        roscomp.shutdown()
        record_globalroute_tmp.close()

if __name__ == "__main__":
    main()

主要是录制成csv文件