Skip to content

Commit

Permalink
feat: init image_projection_based_fusion package
Browse files Browse the repository at this point in the history
Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
  • Loading branch information
yukke42 committed Mar 24, 2022
1 parent 2ab6959 commit 023aa77
Show file tree
Hide file tree
Showing 10 changed files with 640 additions and 0 deletions.
51 changes: 51 additions & 0 deletions perception/image_projection_based_fusion/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
cmake_minimum_required(VERSION 3.8)
project(image_projection_based_fusion)

if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_auto REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)

ament_auto_find_build_dependencies()

include_directories(
include
${OpenCV_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)

ament_auto_add_library(${PROJECT_NAME} SHARED
src/fusion_node.cpp
src/roi_cluster_fusion/node.cpp
src/roi_detected_object_fusion/node.cpp
)
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBRARIES}
${EIGEN3_LIBRARIES}
)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "image_projection_based_fusion::RoiClusterFusionNode"
EXECUTABLE roi_cluster_fusion_node
)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode"
EXECUTABLE roi_detected_object_fusion_node
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
endif()

ament_auto_package(INSTALL_TO_SHARE
launch
)
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_
#define IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>

#include <message_filters/pass_through.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <map>
#include <memory>
#include <string>
#include <vector>

namespace image_projection_based_fusion
{

using autoware_auto_perception_msgs::msg::DetectedObjects;
using tier4_perception_msgs::msg::DetectedObjectsWithFeature;

template <class T>
using SyncPolicy = message_filters::sync_policies::ApproximateTime<
T, DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature,
DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature,
DetectedObjectsWithFeature, DetectedObjectsWithFeature>;
template <class T>
using Sync = message_filters::Synchronizer<SyncPolicy<T>>;

template <class Msg>
class FusionNode : public rclcpp::Node
{
public:
explicit FusionNode(const std::string & node_name, const rclcpp::NodeOptions & options);

protected:
void cameraInfoCallback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const int camera_id);

void fusionCallback(
typename Msg::ConstSharedPtr input_obstacle_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg,
DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg);

virtual void fusionOnSingleImage(const DetectedObjectsWithFeature & input_roi_msg) = 0;

void publish();

int rois_number_{0};
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

// camera_info
std::map<int, sensor_msgs::msg::CameraInfo> camera_info_map_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;

// fusion
typename message_filters::Subscriber<Msg> sub_;
message_filters::PassThrough<DetectedObjectsWithFeature> passthrough_;
std::vector<std::shared_ptr<message_filters::Subscriber<DetectedObjectsWithFeature>>> rois_subs_;
inline void dummyCallback(DetectedObjectsWithFeature::ConstSharedPtr input)
{
passthrough_.add(input);
}
typename std::shared_ptr<Sync<Msg>> sync_ptr_;

// output
// if the type of output message isn't Msg,
// overwrite output_msg_ and pub_ptr_ in derived class.
Msg output_msg_;
typename rclcpp::Publisher<Msg>::SharedPtr pub_ptr_;
};

} // namespace image_projection_based_fusion

#endif // IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_
#define IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_

#include "image_projection_based_fusion/fusion_node.hpp"

#include <memory>

namespace image_projection_based_fusion
{

class RoiClusterFusionNode : public FusionNode<DetectedObjectsWithFeature>
{
public:
explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options);

protected:
void fusionOnSingleImage(const DetectedObjectsWithFeature & input_roi_msg) override;
};

} // namespace image_projection_based_fusion

#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
// Copyright 2022 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_
#define IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_

#include "image_projection_based_fusion/fusion_node.hpp"

#include <memory>

namespace image_projection_based_fusion
{

class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects>
{
public:
explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);

protected:
void fusionOnSingleImage(const DetectedObjectsWithFeature & input_roi_msg) override;
};

} // namespace image_projection_based_fusion

#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
<launch>
<arg name="input/rois_number" default="1"/>
<arg name="input/rois0" default="rois0"/>
<arg name="input/camera_info0" default="/camera_info0"/>
<arg name="input/rois1" default="rois1"/>
<arg name="input/camera_info1" default="/camera_info1"/>
<arg name="input/rois2" default="rois2"/>
<arg name="input/camera_info2" default="/camera_info2"/>
<arg name="input/rois3" default="rois3"/>
<arg name="input/camera_info3" default="/camera_info3"/>
<arg name="input/rois4" default="rois4"/>
<arg name="input/camera_info4" default="/camera_info4"/>
<arg name="input/rois5" default="rois5"/>
<arg name="input/camera_info5" default="/camera_info5"/>
<arg name="input/rois6" default="rois6"/>
<arg name="input/camera_info6" default="/camera_info6"/>
<arg name="input/rois7" default="rois7"/>
<arg name="input/camera_info7" default="/camera_info7"/>
<arg name="input/clusters" default="clusters"/>
<arg name="output/clusters" default="labeled_clusters"/>

<!-- for eval variable-->
<arg name="input_rois_number" default="$(var input/rois_number)"/>

<!-- debug -->
<arg name="debug_mode" default="false"/>
<arg name="input/image0" default="/image_raw0"/>
<arg name="input/image1" default="/image_raw1"/>
<arg name="input/image2" default="/image_raw2"/>
<arg name="input/image3" default="/image_raw3"/>
<arg name="input/image4" default="/image_raw4"/>
<arg name="input/image5" default="/image_raw5"/>
<arg name="input/image6" default="/image_raw6"/>
<arg name="input/image7" default="/image_raw7"/>
<group>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/rois2" to="$(var input/rois2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/camera_info2" to="$(var input/camera_info2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/rois3" to="$(var input/rois3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/camera_info3" to="$(var input/camera_info3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/rois4" to="$(var input/rois4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/camera_info4" to="$(var input/camera_info4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/rois5" to="$(var input/rois5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/camera_info5" to="$(var input/camera_info5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/rois6" to="$(var input/rois6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/camera_info6" to="$(var input/camera_info6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/rois7" to="$(var input/rois7)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/camera_info7" to="$(var input/camera_info7)"/>

<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '1' &quot;)" from="input/image_raw0" to="$(var input/image0)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '2' &quot;)" from="input/image_raw1" to="$(var input/image1)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '3' &quot;)" from="input/image_raw2" to="$(var input/image2)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '4' &quot;)" from="input/image_raw3" to="$(var input/image3)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '5' &quot;)" from="input/image_raw4" to="$(var input/image4)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '6' &quot;)" from="input/image_raw5" to="$(var input/image5)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '7' &quot;)" from="input/image_raw6" to="$(var input/image6)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '8' &quot;)" from="input/image_raw7" to="$(var input/image7)"/>

<node pkg="image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
<param name="rois_number" value="$(var input/rois_number)" />
<remap from="input/obstacle" to="$(var input/clusters)"/>
<remap from="output/obstacle" to="$(var output/clusters)"/>

<!-- debug -->
<param name="debug_mode" value="$(var debug_mode)"/>
</node>
</group>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
<launch>
<arg name="input/rois_number" default="1"/>
<arg name="input/rois0" default="rois0"/>
<arg name="input/camera_info0" default="/camera_info0"/>
<arg name="input/rois1" default="rois1"/>
<arg name="input/camera_info1" default="/camera_info1"/>
<arg name="input/rois2" default="rois2"/>
<arg name="input/camera_info2" default="/camera_info2"/>
<arg name="input/rois3" default="rois3"/>
<arg name="input/camera_info3" default="/camera_info3"/>
<arg name="input/rois4" default="rois4"/>
<arg name="input/camera_info4" default="/camera_info4"/>
<arg name="input/rois5" default="rois5"/>
<arg name="input/camera_info5" default="/camera_info5"/>
<arg name="input/rois6" default="rois6"/>
<arg name="input/camera_info6" default="/camera_info6"/>
<arg name="input/rois7" default="rois7"/>
<arg name="input/camera_info7" default="/camera_info7"/>
<arg name="input/objects" default="objects"/>
<arg name="output/objects" default="fused_objects"/>

<!-- for eval variable-->
<arg name="input_rois_number" default="$(var input/rois_number)"/>

<!-- debug -->
<arg name="debug_mode" default="false"/>
<arg name="input/image0" default="/image_raw0"/>
<arg name="input/image1" default="/image_raw1"/>
<arg name="input/image2" default="/image_raw2"/>
<arg name="input/image3" default="/image_raw3"/>
<arg name="input/image4" default="/image_raw4"/>
<arg name="input/image5" default="/image_raw5"/>
<arg name="input/image6" default="/image_raw6"/>
<arg name="input/image7" default="/image_raw7"/>
<group>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/rois2" to="$(var input/rois2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/camera_info2" to="$(var input/camera_info2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/rois3" to="$(var input/rois3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/camera_info3" to="$(var input/camera_info3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/rois4" to="$(var input/rois4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/camera_info4" to="$(var input/camera_info4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/rois5" to="$(var input/rois5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/camera_info5" to="$(var input/camera_info5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/rois6" to="$(var input/rois6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/camera_info6" to="$(var input/camera_info6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/rois7" to="$(var input/rois7)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/camera_info7" to="$(var input/camera_info7)"/>

<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '1' &quot;)" from="input/image_raw0" to="$(var input/image0)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '2' &quot;)" from="input/image_raw1" to="$(var input/image1)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '3' &quot;)" from="input/image_raw2" to="$(var input/image2)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '4' &quot;)" from="input/image_raw3" to="$(var input/image3)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '5' &quot;)" from="input/image_raw4" to="$(var input/image4)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '6' &quot;)" from="input/image_raw5" to="$(var input/image5)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '7' &quot;)" from="input/image_raw6" to="$(var input/image6)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '8' &quot;)" from="input/image_raw7" to="$(var input/image7)"/>

<node pkg="image_projection_based_fusion" exec="roi_detected_object_fusion_node" name="roi_detected_object_fusion" output="screen">
<!-- <param name="use_iou" value="true"/> -->
<!-- <param name="iou_threshold" value="0.35"/> -->
<param name="rois_number" value="$(var input/rois_number)" />
<remap from="input/obstacle" to="$(var input/objects)"/>
<remap from="output/obstacle" to="$(var output/objects)"/>

<!-- debug -->
<param name="debug_mode" value="$(var debug_mode)"/>
</node>
</group>
</launch>
Loading

0 comments on commit 023aa77

Please sign in to comment.