Skip to content

Commit 88d6a92

Browse files
nnmm1222-takeshi
nnmm
authored and
1222-takeshi
committed
Port pose_initializer to ROS2 (#11)
* CMakeLists.txt & package.xml * Compiles * Works * Use callback instead * Reviewer feedback * Add sequence number check * Review feedback
1 parent 0b8fc36 commit 88d6a92

File tree

8 files changed

+178
-176
lines changed

8 files changed

+178
-176
lines changed

‎localization/autoware_localization_srvs/srv/PoseWithCovarianceStamped.srv

+3
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,6 @@
1+
# A sequence number is included to do primitive error checking
2+
uint32 seq
13
geometry_msgs/PoseWithCovarianceStamped pose_with_cov
24
---
5+
uint32 seq
36
geometry_msgs/PoseWithCovarianceStamped pose_with_cov

‎localization/pose_initializer/CMakeLists.txt

+40-26
Original file line numberDiff line numberDiff line change
@@ -1,47 +1,61 @@
1-
cmake_minimum_required(VERSION 3.0.2)
1+
cmake_minimum_required(VERSION 3.5)
22
project(pose_initializer)
33

4-
add_compile_options(-std=c++14)
4+
### Compile options
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
9+
add_compile_options(-Wall -Wextra -Wpedantic)
10+
add_compile_options(-Wno-unused-parameter)
11+
endif()
12+
13+
### Dependencies
14+
find_package(ament_cmake REQUIRED)
15+
find_package(rclcpp REQUIRED)
16+
find_package(tf2 REQUIRED)
17+
find_package(tf2_ros REQUIRED)
18+
find_package(tf2_geometry_msgs REQUIRED)
19+
find_package(std_msgs REQUIRED)
20+
find_package(geometry_msgs REQUIRED)
21+
find_package(sensor_msgs REQUIRED)
22+
find_package(PCL REQUIRED)
23+
find_package(pcl_conversions REQUIRED)
24+
find_package(autoware_localization_srvs REQUIRED)
25+
26+
### Includes
27+
include_directories(include)
528

6-
find_package(catkin REQUIRED COMPONENTS
7-
roscpp
29+
add_executable(pose_initializer
30+
src/pose_initializer_node.cpp
31+
src/pose_initializer_core.cpp
32+
)
33+
34+
set(POSE_INITIALIZER_DEPENDENCIES
35+
rclcpp
836
tf2
937
tf2_ros
1038
tf2_geometry_msgs
1139
std_msgs
1240
geometry_msgs
1341
sensor_msgs
14-
dynamic_reconfigure
15-
pcl_ros
42+
PCL
1643
pcl_conversions
1744
autoware_localization_srvs
1845
)
1946

20-
catkin_package(
21-
INCLUDE_DIRS include
22-
)
23-
24-
include_directories(include ${catkin_INCLUDE_DIRS})
25-
26-
add_executable(pose_initializer
27-
src/pose_initializer_node.cpp
28-
src/pose_initializer_core.cpp
29-
)
30-
31-
add_dependencies(pose_initializer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
32-
33-
target_link_libraries(pose_initializer ${catkin_LIBRARIES})
47+
ament_target_dependencies(pose_initializer ${POSE_INITIALIZER_DEPENDENCIES})
48+
ament_export_dependencies(${POSE_INITIALIZER_DEPENDENCIES})
3449

3550
install(
36-
TARGETS
37-
pose_initializer
38-
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
39-
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
40-
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
51+
TARGETS pose_initializer
52+
DESTINATION lib/${PROJECT_NAME}
4153
)
4254

4355
install(
4456
DIRECTORY
4557
launch
46-
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
58+
DESTINATION share/${PROJECT_NAME}
4759
)
60+
61+
ament_package()

‎localization/pose_initializer/COLCON_IGNORE

Whitespace-only changes.

‎localization/pose_initializer/include/pose_initializer/pose_initializer_core.h

+32-29
Original file line numberDiff line numberDiff line change
@@ -16,62 +16,65 @@
1616

1717
#pragma once
1818

19+
#include <memory>
1920
#include <string>
2021

21-
#include <ros/ros.h>
22+
#include <rclcpp/rclcpp.hpp>
2223

2324
#include <tf2/transform_datatypes.h>
2425
#include <tf2_ros/transform_listener.h>
2526

26-
#include <geometry_msgs/PoseWithCovarianceStamped.h>
27-
#include <sensor_msgs/PointCloud2.h>
28-
29-
#include <dynamic_reconfigure/server.h>
27+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
28+
#include <sensor_msgs/msg/point_cloud2.hpp>
3029

3130
#include <pcl/point_cloud.h>
3231
#include <pcl/point_types.h>
3332

34-
#include "autoware_localization_srvs/PoseWithCovarianceStamped.h"
33+
#include "autoware_localization_srvs/srv/pose_with_covariance_stamped.hpp"
3534

36-
class PoseInitializer
35+
class PoseInitializer : public rclcpp::Node
3736
{
3837
public:
39-
PoseInitializer(ros::NodeHandle nh, ros::NodeHandle private_nh);
38+
PoseInitializer();
4039
~PoseInitializer();
4140

4241
private:
43-
void callbackMapPoints(const sensor_msgs::PointCloud2::ConstPtr & pointcloud2_msg_ptr);
44-
bool serviceInitial(
45-
autoware_localization_srvs::PoseWithCovarianceStamped::Request & req,
46-
autoware_localization_srvs::PoseWithCovarianceStamped::Response & res);
42+
void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
43+
void serviceInitial(
44+
const std::shared_ptr<autoware_localization_srvs::srv::PoseWithCovarianceStamped::Request> req,
45+
std::shared_ptr<autoware_localization_srvs::srv::PoseWithCovarianceStamped::Response> res);
4746
void callbackInitialPose(
48-
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & pose_cov_msg_ptr);
47+
geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr);
4948
void callbackGNSSPoseCov(
50-
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & pose_cov_msg_ptr);
49+
geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr);
5150

5251
bool getHeight(
53-
const geometry_msgs::PoseWithCovarianceStamped & input_pose_msg,
54-
const geometry_msgs::PoseWithCovarianceStamped::Ptr & output_pose_msg_ptr);
55-
bool callAlignService(
56-
const geometry_msgs::PoseWithCovarianceStamped & msg,
57-
const geometry_msgs::PoseWithCovarianceStamped::Ptr & output_pose_msg_ptr);
58-
59-
ros::NodeHandle nh_;
60-
ros::NodeHandle private_nh_;
52+
const geometry_msgs::msg::PoseWithCovarianceStamped & input_pose_msg,
53+
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr output_pose_msg_ptr);
54+
void callAlignServiceAndPublishResult(
55+
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
6156

62-
ros::Subscriber initial_pose_sub_;
63-
ros::Subscriber gnss_pose_sub_;
64-
ros::Subscriber map_points_sub_;
57+
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
58+
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gnss_pose_sub_;
59+
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
6560

66-
ros::Publisher initial_pose_pub_;
61+
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
6762

68-
ros::ServiceClient ndt_client_;
63+
rclcpp::Client<autoware_localization_srvs::srv::PoseWithCovarianceStamped>::SharedPtr ndt_client_;
6964

70-
ros::ServiceServer gnss_service_;
65+
rclcpp::Service<autoware_localization_srvs::srv::PoseWithCovarianceStamped>::SharedPtr
66+
gnss_service_;
7167

72-
tf2_ros::Buffer tf2_buffer_;
68+
tf2::BufferCore tf2_buffer_;
7369
tf2_ros::TransformListener tf2_listener_;
7470

7571
pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr_;
7672
std::string map_frame_;
73+
74+
// With the currently available facilities for calling a service, there is no
75+
// easy way of detecting whether an answer was received within a reasonable
76+
// amount of time. So, as a sanity check, we check whether a response for the
77+
// previous request was received when a new request is sent.
78+
uint32_t request_id_ = 0;
79+
uint32_t response_id_ = 0;
7780
};

‎localization/pose_initializer/launch/pose_initializer.launch.xml

+1-3
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,11 @@
11
<launch>
2-
32
<arg name="ndt_align_server_name" default="ndt_align_srv"/>
4-
<node pkg="pose_initializer" type="pose_initializer" name="pose_initializer" output="log">
3+
<node pkg="pose_initializer" exec="pose_initializer" name="pose_initializer" output="log">
54
<remap from="initialpose" to="/initialpose" />
65
<remap from="initialpose3d" to="/initialpose3d" />
76
<remap from="gnss_pose_cov" to="/sensing/gnss/pose_with_covariance" />
87
<remap from="pointcloud_map" to="/map/pointcloud_map" />
98
<remap from="ndt_align_srv" to="/localization/pose_estimator/ndt_align_srv" />
109
<param name="use_first_gnss_topic" value="true" />
1110
</node>
12-
1311
</launch>

‎localization/pose_initializer/package.xml

+21-30
Original file line numberDiff line numberDiff line change
@@ -1,34 +1,25 @@
11
<?xml version="1.0"?>
2-
<package>
3-
<name>pose_initializer</name>
4-
<version>0.1.0</version>
5-
<description>The pose_initializer package</description>
6-
<maintainer email="yamato.ando@gmail.com">Yamato Ando</maintainer>
7-
<license>Apache 2</license>
8-
<buildtool_depend>catkin</buildtool_depend>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>pose_initializer</name>
5+
<version>0.1.0</version>
6+
<description>The pose_initializer package</description>
7+
<maintainer email="yamato.ando@gmail.com">Yamato Ando</maintainer>
8+
<license>Apache 2</license>
9+
<buildtool_depend>ament_cmake</buildtool_depend>
910

10-
<build_depend>roscpp</build_depend>
11-
<build_depend>tf2</build_depend>
12-
<build_depend>tf2_ros</build_depend>
13-
<build_depend>tf2_geometry_msgs</build_depend>
14-
<build_depend>std_msgs</build_depend>
15-
<build_depend>geometry_msgs</build_depend>
16-
<build_depend>sensor_msgs</build_depend>
17-
<build_depend>pcl_ros</build_depend>
18-
<build_depend>pcl_conversions</build_depend>
19-
<build_depend>autoware_localization_srvs</build_depend>
11+
<depend>rclcpp</depend>
12+
<depend>tf2</depend>
13+
<depend>tf2_ros</depend>
14+
<depend>tf2_geometry_msgs</depend>
15+
<depend>std_msgs</depend>
16+
<depend>geometry_msgs</depend>
17+
<depend>sensor_msgs</depend>
18+
<depend>libpcl-all-dev</depend>
19+
<depend>pcl_conversions</depend>
20+
<depend>autoware_localization_srvs</depend>
2021

21-
<run_depend>roscpp</run_depend>
22-
<run_depend>tf2</run_depend>
23-
<run_depend>tf2_ros</run_depend>
24-
<run_depend>tf2_geometry_msgs</run_depend>
25-
<run_depend>std_msgs</run_depend>
26-
<run_depend>geometry_msgs</run_depend>
27-
<run_depend>sensor_msgs</run_depend>
28-
<run_depend>pcl_ros</run_depend>
29-
<run_depend>pcl_conversions</run_depend>
30-
<run_depend>autoware_localization_srvs</run_depend>
31-
32-
<export>
33-
</export>
22+
<export>
23+
<build_type>ament_cmake</build_type>
24+
</export>
3425
</package>

0 commit comments

Comments
 (0)