diff --git a/carla_ros_bridge/launch/carla_ros_bridge.launch b/carla_ros_bridge/launch/carla_ros_bridge.launch index e92eb0e1..4b09c2f9 100644 --- a/carla_ros_bridge/launch/carla_ros_bridge.launch +++ b/carla_ros_bridge/launch/carla_ros_bridge.launch @@ -6,7 +6,7 @@ - + - - - - - - - - - - - - - - - - - - - - - - diff --git a/pcl_recorder/launch/pcl_recorder.launch.py b/pcl_recorder/launch/pcl_recorder.launch.py deleted file mode 100644 index d2510032..00000000 --- a/pcl_recorder/launch/pcl_recorder.launch.py +++ /dev/null @@ -1,64 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def launch_enable_autopilot_publisher(context, *args, **kwargs): - topic_name = "/carla/" + launch.substitutions.LaunchConfiguration('role_name').perform(context) + "/enable_autopilot" - return [ - launch.actions.ExecuteProcess( - output="screen", - cmd=["ros2", "topic", "pub", topic_name, - "std_msgs/msg/Bool", "{'data': True}", "--qos-durability", "transient_local"], - name='topic_pub_enable_autopilot')] - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='host', - default_value='localhost' - ), - launch.actions.DeclareLaunchArgument( - name='port', - default_value='2000' - ), - launch.actions.DeclareLaunchArgument( - name='role_name', - default_value='ego_vehicle' - ), - launch.actions.OpaqueFunction(function=launch_enable_autopilot_publisher), - launch_ros.actions.Node( - package='pcl_recorder', - executable='pcl_recorder_node', - name='pcl_recorder_node', - output='screen', - parameters=[ - { - 'use_sim_time': True - }, - { - 'role_name': launch.substitutions.LaunchConfiguration('role_name') - }, - ] - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_ros_bridge'), 'carla_ros_bridge_with_example_ego_vehicle.launch.py') - ), - launch_arguments={ - 'host': launch.substitutions.LaunchConfiguration('host'), - 'port': launch.substitutions.LaunchConfiguration('port'), - 'role_name': launch.substitutions.LaunchConfiguration('role_name') - }.items() - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/pcl_recorder/package.xml b/pcl_recorder/package.xml deleted file mode 100644 index 1473499d..00000000 --- a/pcl_recorder/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - pcl_recorder - 0.0.0 - The pcl_recorder package - CARLA Simulator Team - MIT - - - - catkin - roscpp - roslaunch - roscpp - roscpp - rostopic - pcl_ros - pcl_ros - pcl_ros - - - rclcpp - ament_cmake - - pcl_conversions - sensor_msgs - pcl_conversions - sensor_msgs - pcl_conversions - sensor_msgs - carla_ros_bridge - - - catkin - ament_cmake - - diff --git a/pcl_recorder/src/PclRecorder.cpp b/pcl_recorder/src/PclRecorder.cpp deleted file mode 100644 index 4f0ad0ee..00000000 --- a/pcl_recorder/src/PclRecorder.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (c) 2019 Intel Corporation - * - * This work is licensed under the terms of the MIT license. - * For a copy, see . - */ -#include "PclRecorder.h" -#include -#include -#include -#include - -PclRecorder::PclRecorder() -{ - tfListener = new tf2_ros::TransformListener(tf_buffer_); - - if (mkdir("/tmp/pcl_capture", 0777) == -1) { - ROS_WARN("Could not create directory!"); - } - - // Create a ROS subscriber for the input point cloud - std::string roleName; - if (!ros::param::get("~role_name", roleName)) { - roleName = "ego_vehicle"; - } - sub = nh.subscribe("/carla/" + roleName + "/lidar", 1, &PclRecorder::callback, this); -} - -void PclRecorder::callback(const boost::shared_ptr& cloud) -{ - if ((cloud->width * cloud->height) == 0) { - return; - } - - std::stringstream ss; - ss << "/tmp/pcl_capture/capture" << cloud->header.stamp << ".pcd"; - - ROS_INFO ("Received %d data points. Storing in %s", - (int)cloud->width * cloud->height, - ss.str().c_str()); - - Eigen::Affine3d transform; - try { - transform = tf2::transformToEigen (tf_buffer_.lookupTransform(fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration(1))); - - pcl::PointCloud pclCloud; - pcl::fromPCLPointCloud2(*cloud, pclCloud); - - pcl::PointCloud transformedCloud; - pcl::transformPointCloud (pclCloud, transformedCloud, transform); - - pcl::PCDWriter writer; - writer.writeBinary(ss.str(), transformedCloud); - } - catch (tf2::TransformException &ex) - { - ROS_WARN("Could NOT transform: %s", ex.what()); - } -} diff --git a/pcl_recorder/src/PclRecorderROS2.cpp b/pcl_recorder/src/PclRecorderROS2.cpp deleted file mode 100644 index 62f6864e..00000000 --- a/pcl_recorder/src/PclRecorderROS2.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright (c) 2019-2020 Intel Corporation - * - * This work is licensed under the terms of the MIT license. - * For a copy, see . - */ -#include -#include - -#include -#include - -#include "PclRecorderROS2.h" - -PclRecorderROS2::PclRecorderROS2() : Node("pcl_recorder") -{ - tf_buffer_ = std::make_unique(this->get_clock()); - tfListener = new tf2_ros::TransformListener(*tf_buffer_); - - if (mkdir("/tmp/pcl_capture", 0777) == -1) { - RCLCPP_WARN(this->get_logger(), "Could not create directory!"); - } - - // Create a ROS subscriber for the input point cloud - std::string roleName; - if (!this->get_parameter("role_name", roleName)) { - roleName = "ego_vehicle"; - } - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - sub = this->create_subscription("/carla/" + roleName + "/lidar", 10, std::bind(&PclRecorderROS2::callback, this, std::placeholders::_1), sub_opt); -} - -void PclRecorderROS2::callback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) -{ - if ((cloud->width * cloud->height) == 0) { - return; - } - - std::stringstream ss; - ss << "/tmp/pcl_capture/capture" << cloud->header.stamp.sec << cloud->header.stamp.nanosec << ".pcd"; - - - RCLCPP_INFO (this->get_logger(), "Received %d data points. Storing in %s", - (int)cloud->width * cloud->height, - ss.str().c_str()); - - Eigen::Affine3d transform; - try { - transform = tf2::transformToEigen (tf_buffer_->lookupTransform(fixed_frame_, cloud->header.frame_id, cloud->header.stamp, rclcpp::Duration::from_seconds(1))); - - pcl::PointCloud pclCloud; - pcl::fromROSMsg(*cloud, pclCloud); - - pcl::PointCloud transformedCloud; - pcl::transformPointCloud (pclCloud, transformedCloud, transform); - - pcl::PCDWriter writer; - writer.writeBinary(ss.str(), transformedCloud); - } - catch (tf2::TransformException &ex) - { - RCLCPP_WARN(this->get_logger(), "Could NOT transform: %s", ex.what()); - } -} diff --git a/pcl_recorder/src/main.cpp b/pcl_recorder/src/main.cpp deleted file mode 100644 index c00dcb0e..00000000 --- a/pcl_recorder/src/main.cpp +++ /dev/null @@ -1,15 +0,0 @@ -/* - * Copyright (c) 2019 Intel Corporation - * - * This work is licensed under the terms of the MIT license. - * For a copy, see . - */ -#include -#include "PclRecorder.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "pcl_recorder"); - PclRecorder pclRecorder; - ros::spin(); -}; diff --git a/pcl_recorder/src/mainROS2.cpp b/pcl_recorder/src/mainROS2.cpp deleted file mode 100644 index 93c2a286..00000000 --- a/pcl_recorder/src/mainROS2.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/* - * Copyright (c) 2019 Intel Corporation - * - * This work is licensed under the terms of the MIT license. - * For a copy, see . - */ -#include "PclRecorderROS2.h" - -#include "rclcpp/rclcpp.hpp" - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto recorder = std::make_shared(); - executor.add_node(recorder); - executor.spin(); - rclcpp::shutdown(); - return 0; -}