fix failure transferring sensor data from carla to carma #19
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,157 +1,125 @@ | ||
| #include "carla_sensors_integration_node.h" | ||
|
|
||
| #include<ros/ros.h> | ||
|
|
||
| namespace carla_sensors { | ||
| void CarlaSensorsNode::initialize() { | ||
| nh_.reset(new ros::CARMANodeHandle()); | ||
| points_raw_pub_ = nh_ -> advertise < sensor_msgs::PointCloud2 > ("points_raw", 1); | ||
| image_raw_pub_ = nh_ -> advertise < sensor_msgs::Image > ("image_raw", 1); | ||
| image_color_pub_ = nh_ -> advertise < sensor_msgs::Image > ("image_color", 1); | ||
| image_rect_pub_ = nh_ -> advertise < sensor_msgs::Image > ("image_rect", 1); | ||
| camera_info_pub_ = nh_ -> advertise < sensor_msgs::CameraInfo > ("camera_info", 1); | ||
| gnss_fixed_fused_pub_ = nh_ -> advertise < gps_common::GPSFix > ("gnss_fix_fused", 1); | ||
|
|
||
| pnh_.reset(new ros::CARMANodeHandle("~")); | ||
| pnh_ -> getParam("role_name", carla_vehicle_role_); | ||
| pnh_ -> getParam("object_detection_stream", object_detection_stream_enabled); | ||
| pnh_ -> getParam("localization_stream", localization_stream_enabled); | ||
| pnh_ -> getParam("carla_lidar_stream", carla_lidar_stream_enabled); | ||
| pnh_ -> getParam("carla_camera_stream", carla_camera_stream_enabled); | ||
| pnh_ -> getParam("carla_gnss_stream", carla_gnss_stream_enabled); | ||
| //Subscribers | ||
|
|
||
| /*Lidar*/ | ||
| if (!carla_lidar_stream_enabled) { | ||
| ROS_ERROR_STREAM("CARLA LIDAR data stream is disabled"); | ||
|
There was a problem hiding this comment. I think error is probably the wrong log level for this, debug seems more appropriate. Nothing has necessarily gone wrong if we've disabled the simulated LIDAR. |
||
| } else if (localization_stream_enabled && object_detection_stream_enabled) { | ||
| throw std::invalid_argument("CARLA LIDAR sensor and both ground truth data streams cannot be enabled at the same time"); | ||
| } else { | ||
|
There was a problem hiding this comment. Do we need a catch-all somewhere to check if we've got a minimal set of data to run with? If we turn off simulated lidar/gnss AND the localization truth data, we've got nothing to work with at all. I imagine CARMA Platform downstream of these nodes would complain in that case, but the error would be further away from the source than if we were to check that somewhere here. |
||
| point_cloud_sub_ = nh_ -> subscribe < sensor_msgs::PointCloud2 > ("/carla/" + carla_vehicle_role_ + "/lidar/lidar/point_cloud", 10, & CarlaSensorsNode::point_cloud_cb, this); | ||
| } | ||
|
|
||
| /*Camera*/ | ||
| if (!carla_camera_stream_enabled) { | ||
| ROS_ERROR_STREAM("CARLA camera data stream is disabled"); | ||
|
There was a problem hiding this comment. Similar to above RE: debug level |
||
| } else if (carla_camera_stream_enabled && object_detection_stream_enabled) { | ||
|
There was a problem hiding this comment. Minor: The check here again on carla_camera_stream_enabled is redundant and can be removed, since we're already in the else-branch of an |
||
| throw std::invalid_argument("CARLA Camera sensor and ground truth object detection cannot be enabled at the same time"); | ||
| } else { | ||
| image_raw_sub_ = nh_ -> subscribe < sensor_msgs::Image > ("/carla/" + carla_vehicle_role_ + "/camera/rgb/front/image", 10, & CarlaSensorsNode::image_raw_cb, this); | ||
| image_color_sub_ = nh_ -> subscribe < sensor_msgs::Image > ("/carla/" + carla_vehicle_role_ + "/camera/rgb/front/image_color", 10, & CarlaSensorsNode::image_color_cb, this); | ||
| image_rect_sub_ = nh_ -> subscribe < sensor_msgs::Image > ("/carla/" + carla_vehicle_role_ + "/camera/rgb/front/image_rect", 10, & CarlaSensorsNode::image_rect_cb, this); | ||
| } | ||
| /*GNSS*/ | ||
| if (!carla_gnss_stream_enabled) { | ||
| ROS_ERROR_STREAM("CARLA camera data stream is disabled"); | ||
|
|
||
| } else if (carla_gnss_stream_enabled && localization_stream_enabled) { | ||
|
There was a problem hiding this comment. first logical term redundant |
||
| throw std::invalid_argument("CARLA GNSS sensor and ground truth localization cannot be enabled at the same time"); | ||
| } else { | ||
| gnss_fixed_fused_sub_ = nh_ -> subscribe < sensor_msgs::NavSatFix > ("/carla/" + carla_vehicle_role_ + "/gnss/novatel_gnss/fix", 10, & CarlaSensorsNode::gnss_fixed_fused_cb, this); | ||
| } | ||
| camera_info_sub_ = nh_ -> subscribe < sensor_msgs::CameraInfo > ("/carla/" + carla_vehicle_role_ + "/camera/rgb/front/camera_info", 10, & CarlaSensorsNode::camera_info_cb, this); | ||
|
|
||
| } | ||
|
|
||
| void CarlaSensorsNode::run() { | ||
| initialize(); | ||
| ros::CARMANodeHandle::spin(); | ||
| } | ||
|
|
||
| void CarlaSensorsNode::point_cloud_cb(sensor_msgs::PointCloud2 point_cloud) { | ||
| if (point_cloud.data.size() == 0) { | ||
| throw std::invalid_argument(" Invalid LIDAR Point Cloud Data"); | ||
| exit(0); | ||
| } | ||
| if (point_cloud.header.stamp == point_cloud_msg.header.stamp && point_cloud_msg.header.seq != 0) { | ||
| return; | ||
| } | ||
|
|
||
| carla_worker_.point_cloud_cb(point_cloud); | ||
|
|
||
| point_cloud_msg = carla_worker_.get_lidar_msg(); | ||
| points_raw_pub_.publish(point_cloud_msg); | ||
|
|
||
| } | ||
|
|
||
| void CarlaSensorsNode::image_raw_cb(sensor_msgs::Image image_raw) { | ||
| if (image_raw.data.size() == 0) { | ||
| throw std::invalid_argument("Invalid image data"); | ||
| } | ||
| carla_worker_.image_raw_cb(image_raw); | ||
| image_raw_msg = carla_worker_.get_image_raw_msg(); | ||
| image_raw_pub_.publish(image_raw_msg); | ||
|
|
||
| } | ||
|
|
||
| void CarlaSensorsNode::image_color_cb(sensor_msgs::Image image_color) { | ||
|
|
||
| if (image_color.data.size() == 0) { | ||
| throw std::invalid_argument("Invalid image data"); | ||
| } | ||
|
|
||
| carla_worker_.image_color_cb(image_color); | ||
| image_color_msg = carla_worker_.get_image_color_msg(); | ||
| image_color_pub_.publish(image_color_msg); | ||
|
|
||
| } | ||
|
|
||
| void CarlaSensorsNode::image_rect_cb(sensor_msgs::Image image_rect) { | ||
| if (image_rect.data.size() == 0) { | ||
| throw std::invalid_argument("Invalid image data"); | ||
| } | ||
|
|
||
| carla_worker_.image_rect_cb(image_rect); | ||
| image_rect_msg = carla_worker_.get_image_rect_msg(); | ||
| image_rect_pub_.publish(image_rect_msg); | ||
|
|
||
| } | ||
|
|
||
| void CarlaSensorsNode::camera_info_cb(sensor_msgs::CameraInfo camera_info) { | ||
|
|
||
| carla_worker_.camera_info_cb(camera_info); | ||
|
|
||
| camera_info_msg = carla_worker_.get_camera_info(); | ||
| camera_info_pub_.publish(camera_info_msg); | ||
|
|
||
| } | ||
|
|
||
| void CarlaSensorsNode::gnss_fixed_fused_cb(sensor_msgs::NavSatFix gnss_fixed) { | ||
|
|
||
| carla_worker_.gnss_fixed_fused_cb(gnss_fixed); | ||
| gnss_fixed_msg = carla_worker_.get_gnss_fixed_msg(); | ||
| gnss_fixed_fused_pub_.publish(gnss_fixed_msg); | ||
|
|
||
| } | ||
| } | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is this really the right syntax? I'm a bit unfamiliar, but anything
== truereads as weird to me. Is this doing raw string comparison instead of converting the arg into a boolean? I'd have to look through the launch files elsewhere to see if this pattern is used, or somewhere in the roslaunch docs.Not a huge deal if it works, but just a bit weird.