Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(ad_api_adaptors): componentize nodes #7022

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 12 additions & 3 deletions system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,21 @@ project(ad_api_adaptors)
find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_executable(initial_pose_adaptor
ament_auto_add_library(${PROJECT_NAME} SHARED
src/initial_pose_adaptor.cpp
src/routing_adaptor.cpp
)

ament_auto_add_executable(routing_adaptor
src/routing_adaptor.cpp
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "ad_api_adaptors::InitialPoseAdaptor"
EXECUTABLE initial_pose_adaptor_node
EXECUTOR MultiThreadedExecutor
)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "ad_api_adaptors::RoutingAdaptor"
EXECUTABLE routing_adaptor_node
EXECUTOR SingleThreadedExecutor
)

ament_auto_package(INSTALL_TO_SHARE config launch)
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

<group>
<push-ros-namespace namespace="default_ad_api/helpers"/>
<node pkg="ad_api_adaptors" exec="initial_pose_adaptor" name="initial_pose_adaptor">
<node pkg="ad_api_adaptors" exec="initial_pose_adaptor_node">
<param from="$(find-pkg-share ad_api_adaptors)/config/initial_pose.param.yaml"/>
<param name="map_height_fitter.map_loader_name" value="/map/pointcloud_map_loader"/>
<param name="map_height_fitter.target" value="$(var rviz_initial_pose_auto_fix_target)"/>
Expand All @@ -12,7 +12,7 @@
<remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
<remap from="~/vector_map" to="/map/vector_map"/>
</node>
<node pkg="ad_api_adaptors" exec="routing_adaptor" name="routing_adaptor">
<node pkg="ad_api_adaptors" exec="routing_adaptor_node">
<remap from="~/input/fixed_goal" to="/planning/mission_planning/goal"/>
<remap from="~/input/rough_goal" to="/rviz/routing/rough_goal"/>
<remap from="~/input/reroute" to="/rviz/routing/reroute"/>
Expand Down
1 change: 1 addition & 0 deletions system/default_ad_api_helpers/ad_api_adaptors/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
<depend>component_interface_utils</depend>
<depend>map_height_fitter</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,8 @@ std::array<double, 36> get_covariance_parameter(rclcpp::Node * node, const std::
return array;
}

InitialPoseAdaptor::InitialPoseAdaptor() : Node("initial_pose_adaptor"), fitter_(this)
InitialPoseAdaptor::InitialPoseAdaptor(const rclcpp::NodeOptions & options)
: Node("initial_pose_adaptor", options), fitter_(this)
{
rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance");
sub_initial_pose_ = create_subscription<PoseWithCovarianceStamped>(
Expand All @@ -61,13 +62,5 @@ void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstS

} // namespace ad_api_adaptors

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::executors::MultiThreadedExecutor executor;
auto node = std::make_shared<ad_api_adaptors::InitialPoseAdaptor>();
executor.add_node(node);
executor.spin();
executor.remove_node(node);
rclcpp::shutdown();
}
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::InitialPoseAdaptor)
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ namespace ad_api_adaptors
class InitialPoseAdaptor : public rclcpp::Node
{
public:
InitialPoseAdaptor();
explicit InitialPoseAdaptor(const rclcpp::NodeOptions & options);

private:
using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,8 @@
namespace ad_api_adaptors
{

RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor")
RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options)
: Node("routing_adaptor", options)
{
using std::placeholders::_1;

Expand Down Expand Up @@ -110,13 +111,5 @@ void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose)

} // namespace ad_api_adaptors

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
auto node = std::make_shared<ad_api_adaptors::RoutingAdaptor>();
executor.add_node(node);
executor.spin();
executor.remove_node(node);
rclcpp::shutdown();
}
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::RoutingAdaptor)
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ namespace ad_api_adaptors
class RoutingAdaptor : public rclcpp::Node
{
public:
RoutingAdaptor();
explicit RoutingAdaptor(const rclcpp::NodeOptions & options);

private:
using PoseStamped = geometry_msgs::msg::PoseStamped;
Expand Down
Loading