diff --git a/.github/workflows/noetic-ci.yml b/.github/workflows/noetic-ci.yml index 983526f..873efb4 100644 --- a/.github/workflows/noetic-ci.yml +++ b/.github/workflows/noetic-ci.yml @@ -8,8 +8,6 @@ jobs: steps: - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: run industrial_ci uses: 'ros-industrial/industrial_ci@master' env: @@ -22,8 +20,6 @@ jobs: steps: - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: run industrial_ci uses: 'ros-industrial/industrial_ci@master' env: @@ -36,8 +32,6 @@ jobs: steps: - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: run industrial_ci uses: 'ros-industrial/industrial_ci@master' env: @@ -49,8 +43,6 @@ jobs: steps: - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: run industrial_ci uses: 'ros-industrial/industrial_ci@master' env: @@ -64,8 +56,6 @@ jobs: steps: - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: run industrial_ci uses: 'ros-industrial/industrial_ci@master' env: @@ -92,8 +82,6 @@ jobs: steps: - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: run industrial_ci uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/noetic-docker-build-check.yml b/.github/workflows/noetic-docker-build-check.yml index 88fd007..4482fa0 100644 --- a/.github/workflows/noetic-docker-build-check.yml +++ b/.github/workflows/noetic-docker-build-check.yml @@ -13,17 +13,10 @@ jobs: run: rm -rf /opt/hostedtoolcache - name: checkout uses: actions/checkout@v2 - with: - ref: ${{ github.head_ref }} - name: set up QEMU uses: docker/setup-qemu-action@v1 - name: set up Docker Buildx uses: docker/setup-buildx-action@v1 - - name: login to DockerHub - uses: docker/login-action@v1 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - name: build check uses: docker/build-push-action@v2 with: diff --git a/include/tracker_with_cloud_node/tracker_with_cloud_node.h b/include/tracker_with_cloud_node/tracker_with_cloud_node.h index f9f8d7e..6b98ae5 100644 --- a/include/tracker_with_cloud_node/tracker_with_cloud_node.h +++ b/include/tracker_with_cloud_node/tracker_with_cloud_node.h @@ -93,9 +93,10 @@ class TrackerWithCloudNode pcl::PointCloud::Ptr& detection_cloud_raw); void processPointsWithMask(const pcl::PointCloud::Ptr& cloud, const sensor_msgs::Image& mask, pcl::PointCloud::Ptr& detection_cloud_raw); - pcl::PointCloud::Ptr msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); + pcl::PointCloud::Ptr downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); pcl::PointCloud::Ptr cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, - const std_msgs::Header& header); + const std::string& source_frame, + const std::string& target_frame, const ros::Time& stamp); pcl::PointCloud::Ptr euclideanClusterExtraction(const pcl::PointCloud::Ptr& cloud); void createBoundingBox(vision_msgs::Detection3DArray& detections3d_msg, const pcl::PointCloud::Ptr& cloud, diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp index 891e07d..c8f5668 100644 --- a/src/tracker_with_cloud_node.cpp +++ b/src/tracker_with_cloud_node.cpp @@ -50,14 +50,17 @@ void TrackerWithCloudNode::syncCallback(const sensor_msgs::CameraInfo::ConstPtr& ros::Time current_call_time = ros::Time::now(); ros::Duration callback_interval = current_call_time - last_call_time_; last_call_time_ = current_call_time; + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + downsampled_cloud = downsampleCloudMsg(cloud_msg); pcl::PointCloud::Ptr transformed_cloud; + transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cloud_msg->header.frame_id, cam_model_.tfFrame(), + cloud_msg->header.stamp); vision_msgs::Detection3DArray detections3d_msg; sensor_msgs::PointCloud2 detection_cloud_msg; visualization_msgs::MarkerArray marker_array_msg; cam_model_.fromCameraInfo(camera_info_msg); - transformed_cloud = msg2TransformedCloud(cloud_msg); projectCloud(transformed_cloud, yolo_result_msg, cloud_msg->header, detections3d_msg, detection_cloud_msg); marker_array_msg = createMarkerArray(detections3d_msg, callback_interval.toSec()); @@ -90,7 +93,8 @@ void TrackerWithCloudNode::projectCloud(const pcl::PointCloud::Pt if (!detection_cloud_raw->points.empty()) { - pcl::PointCloud::Ptr detection_cloud = cloud2TransformedCloud(detection_cloud_raw, header); + pcl::PointCloud::Ptr detection_cloud = + cloud2TransformedCloud(detection_cloud_raw, cam_model_.tfFrame(), header.frame_id, header.stamp); pcl::PointCloud::Ptr closest_detection_cloud = euclideanClusterExtraction(detection_cloud); createBoundingBox(detections3d_msg, closest_detection_cloud, yolo_result_msg->detections.detections[i].results); combine_detection_cloud += *closest_detection_cloud; @@ -151,36 +155,28 @@ void TrackerWithCloudNode::processPointsWithMask(const pcl::PointCloud::Ptr -TrackerWithCloudNode::msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) +TrackerWithCloudNode::downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { - pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud); - sensor_msgs::PointCloud2 transformed_cloud_msg; - - try - { - geometry_msgs::TransformStamped tf = - tf_buffer_->lookupTransform(cam_model_.tfFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp); - pcl_ros::transformPointCloud(cam_model_.tfFrame(), *cloud_msg, transformed_cloud_msg, *tf_buffer_); - pcl::fromROSMsg(transformed_cloud_msg, *transformed_cloud); - } - catch (tf2::TransformException& e) - { - ROS_WARN("%s", e.what()); - } - - return transformed_cloud; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*cloud_msg, *cloud); + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + pcl::VoxelGrid voxel_grid; + voxel_grid.setInputCloud(cloud); + voxel_grid.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + voxel_grid.filter(*downsampled_cloud); + return downsampled_cloud; } pcl::PointCloud::Ptr TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, - const std_msgs::Header& header) + const std::string& source_frame, const std::string& target_frame, + const ros::Time& stamp) { pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud); try { - geometry_msgs::TransformStamped tf = - tf_buffer_->lookupTransform(header.frame_id, cam_model_.tfFrame(), header.stamp); + geometry_msgs::TransformStamped tf = tf_buffer_->lookupTransform(target_frame, source_frame, stamp); pcl_ros::transformPointCloud(*cloud, *transformed_cloud, tf.transform); } catch (tf2::TransformException& e) @@ -194,12 +190,6 @@ TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud::Ptr TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud::Ptr& cloud) { - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - pcl::VoxelGrid voxel_grid; - voxel_grid.setInputCloud(cloud); - voxel_grid.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); - voxel_grid.filter(*downsampled_cloud); - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; @@ -208,7 +198,7 @@ TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud::max(); @@ -219,7 +209,7 @@ TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud); for (const auto& indice : cluster.indices) { - cloud_cluster->push_back((*downsampled_cloud)[indice]); + cloud_cluster->push_back((*cloud)[indice]); } Eigen::Vector4f centroid;