Port awapi_awiv_adapter to ROS2 (autowarefoundation#51)

* Port autoware_api_msgs to ROS2 * Integrate topic_tools * Don't inherit from Node, convert to milliseconds * Remove _node suffix from non-node loggers Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
taikitanaka3 · Dec 3, 2021 · db77d4d · db77d4d
1 parent d79c4f9
commit db77d4d
Show file tree

Hide file tree

Showing 19 changed files with 561 additions and 500 deletions.
diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt
@@ -1,36 +1,17 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(awapi_awiv_adapter)
 
-add_compile_options(-std=c++14)
-
-find_package(catkin REQUIRED COMPONENTS
-  message_generation
-  message_runtime
-  roscpp
-  autoware_api_msgs
-  autoware_system_msgs
-  autoware_planning_msgs
-  autoware_control_msgs
-  autoware_vehicle_msgs
-  diagnostic_msgs
-  geometry_msgs
-  pacmod_msgs
-  std_msgs
-  sensor_msgs
-  tf2
-  tf2_geometry_msgs
-)
-
-catkin_package(
-  INCLUDE_DIRS include
-)
+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(-Wno-unused-parameter -Wall -Wextra -Wpedantic -Werror)
+endif()
 
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-add_executable(awapi_awiv_adapter
+ament_auto_add_executable(awapi_awiv_adapter
   src/awapi_awiv_adapter_node.cpp
   src/awapi_awiv_adapter_core.cpp
   src/awapi_vehicle_state_publisher.cpp
@@ -41,30 +22,4 @@ add_executable(awapi_awiv_adapter
   src/awapi_autoware_util.cpp
 )
 
-target_link_libraries(awapi_awiv_adapter
-  ${catkin_LIBRARIES}
-)
-
-add_dependencies(awapi_awiv_adapter
-  ${catkin_EXPORTED_TARGETS}
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-install(
-  DIRECTORY
-    launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(TARGETS awapi_awiv_adapter
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-endif()
+ament_auto_package(INSTALL_TO_SHARE launch)
diff --git a/awapi/awapi_awiv_adapter/COLCON_IGNORE b/awapi/awapi_awiv_adapter/COLCON_IGNORE
diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md
@@ -117,21 +117,21 @@
 ### /awapi/autoware/put/emergency
 
 - send emergency signal
-- MessageType: std_msgs/Bool
+- MessageType: autoware_control_msgs/EmergencyMode
 - <font color="Cyan">**To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.**</font>
 
-| ✓   | type          | name | unit | note |
-| --- | :------------ | :--- | :--- | :--- |
-| ✓   | std_msgs/Bool |      |      |      |
+| ✓   | type                                | name | unit | note |
+| --- | :---------------------------------- | :--- | :--- | :--- |
+| ✓   | autoware_control_msgs/EmergencyMode |      |      |      |
 
 ### /awapi/autoware/put/engage
 
 - send engage signal (both of autoware/engage and vehicle/engage)
-- MessageType: std_msgs/Bool
+- MessageType: autoware_control_msgs/EngageMode
 
-| ✓   | type          | name | unit | note |
-| --- | :------------ | :--- | :--- | :--- |
-| ✓   | std_msgs/Bool |      |      |      |
+| ✓   | type                             | name | unit | note |
+| --- | :------------------------------- | :--- | :--- | :--- |
+| ✓   | autoware_control_msgs/EngageMode |      |      |      |
 
 ### /awapi/autoware/put/route
 

diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h
@@ -14,63 +14,62 @@
  * limitations under the License.
  */
 
-#include <autoware_api_msgs/AwapiAutowareStatus.h>
+#include <rclcpp/rclcpp.hpp>
+
 #include <awapi_awiv_adapter/awapi_autoware_util.h>
+#include <autoware_api_msgs/msg/awapi_autoware_status.hpp>
 
 namespace autoware_api
 {
 class AutowareIvAutowareStatePublisher
 {
 public:
-  AutowareIvAutowareStatePublisher();
+  AutowareIvAutowareStatePublisher(rclcpp::Node& node);
   void statePublisher(const AutowareInfo & aw_info);
 
 private:
-  // node handle
-  ros::NodeHandle nh_;
-  ros::NodeHandle pnh_;
-
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
   // publisher
-  ros::Publisher pub_state_;
+  rclcpp::Publisher<autoware_api_msgs::msg::AwapiAutowareStatus>::SharedPtr pub_state_;
 
   // parameter
 
   /* parameter for judging goal now */
   bool arrived_goal_;
-  autoware_system_msgs::AutowareState::_state_type prev_state_;
+  autoware_system_msgs::msg::AutowareState::_state_type prev_state_;
 
   /* parameter for judging diag leaf */
   std::set<std::string> diag_name_set_;
 
   void getAutowareStateInfo(
-    const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
+    const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
   void getControlModeInfo(
-    const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
+    const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr & control_mode_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
   void getGateModeInfo(
-    const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
+    const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
   void getIsEmergencyInfo(
-    const std_msgs::Bool::ConstPtr & is_emergency_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
+    const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
   void getStopReasonInfo(
-    const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
+    const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
   void getDiagInfo(
-    const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
-
+    const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & diag_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
   void getGlobalRptInfo(
-    const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr,
-    autoware_api_msgs::AwapiAutowareStatus * status);
+    const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr,
+    autoware_api_msgs::msg::AwapiAutowareStatus * status);
 
-  bool isGoal(const autoware_system_msgs::AutowareState::ConstPtr & autoware_state);
-  std::vector<diagnostic_msgs::DiagnosticStatus> extractLeafDiag(
-    const std::vector<diagnostic_msgs::DiagnosticStatus> & diag_vec);
+  bool isGoal(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state);
+  std::vector<diagnostic_msgs::msg::DiagnosticStatus> extractLeafDiag(
+    const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & diag_vec);
   std::string splitStringByLastSlash(const std::string & str);
-  void updateDiagNameSet(const std::vector<diagnostic_msgs::DiagnosticStatus> & diag_vec);
-  bool isLeaf(const diagnostic_msgs::DiagnosticStatus & diag);
+  void updateDiagNameSet(const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & diag_vec);
+  bool isLeaf(const diagnostic_msgs::msg::DiagnosticStatus & diag);
 };
 
 }  // namespace autoware_api
diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h
@@ -19,82 +19,56 @@
 #ifndef AWAPI_AUTOWARE_UTIL_H
 #define AWAPI_AUTOWARE_UTIL_H
 
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/TwistStamped.h>
-#include <pacmod_msgs/GlobalRpt.h>
-#include <ros/ros.h>
-#include <sensor_msgs/NavSatFix.h>
-#include <std_msgs/Bool.h>
-#include <std_msgs/Float32.h>
 #include <tf2/utils.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <pacmod_msgs/msg/global_rpt.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float32.hpp>
 
-#include <autoware_control_msgs/GateMode.h>
-#include <autoware_planning_msgs/Path.h>
-#include <autoware_planning_msgs/StopReasonArray.h>
-#include <autoware_planning_msgs/Trajectory.h>
-#include <autoware_system_msgs/AutowareState.h>
-#include <autoware_vehicle_msgs/ControlMode.h>
-#include <autoware_vehicle_msgs/ShiftStamped.h>
-#include <autoware_vehicle_msgs/Steering.h>
-#include <autoware_vehicle_msgs/TurnSignal.h>
-#include <autoware_vehicle_msgs/VehicleCommand.h>
-#include <diagnostic_msgs/DiagnosticArray.h>
+#include <autoware_control_msgs/msg/emergency_mode.hpp>
+#include <autoware_control_msgs/msg/gate_mode.hpp>
+#include <autoware_planning_msgs/msg/path.hpp>
+#include <autoware_planning_msgs/msg/stop_reason_array.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_system_msgs/msg/autoware_state.hpp>
+#include <autoware_vehicle_msgs/msg/control_mode.hpp>
+#include <autoware_vehicle_msgs/msg/shift_stamped.hpp>
+#include <autoware_vehicle_msgs/msg/steering.hpp>
+#include <autoware_vehicle_msgs/msg/turn_signal.hpp>
+#include <autoware_vehicle_msgs/msg/vehicle_command.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 
 namespace autoware_api
 {
 struct AutowareInfo
 {
-  std::shared_ptr<geometry_msgs::PoseStamped> current_pose_ptr;
-  autoware_vehicle_msgs::Steering::ConstPtr steer_ptr;
-  autoware_vehicle_msgs::VehicleCommand::ConstPtr vehicle_cmd_ptr;
-  autoware_vehicle_msgs::TurnSignal::ConstPtr turn_signal_ptr;
-  geometry_msgs::TwistStamped::ConstPtr twist_ptr;
-  autoware_vehicle_msgs::ShiftStamped::ConstPtr gear_ptr;
-  std_msgs::Float32::ConstPtr battery_ptr;
-  sensor_msgs::NavSatFix::ConstPtr nav_sat_ptr;
-  autoware_system_msgs::AutowareState::ConstPtr autoware_state_ptr;
-  autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr;
-  autoware_control_msgs::GateMode::ConstPtr gate_mode_ptr;
-  std_msgs::Bool::ConstPtr is_emergency_ptr;
-  autoware_planning_msgs::StopReasonArray::ConstPtr stop_reason_ptr;
-  diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr;
-  pacmod_msgs::GlobalRpt::ConstPtr global_rpt_ptr;
-  std_msgs::Bool::ConstPtr lane_change_available_ptr;
-  std_msgs::Bool::ConstPtr lane_change_ready_ptr;
-  autoware_planning_msgs::Path::ConstPtr lane_change_candidate_ptr;
-  std_msgs::Bool::ConstPtr obstacle_avoid_ready_ptr;
-  autoware_planning_msgs::Trajectory::ConstPtr obstacle_avoid_candidate_ptr;
+  std::shared_ptr<geometry_msgs::msg::PoseStamped> current_pose_ptr;
+  autoware_vehicle_msgs::msg::Steering::ConstSharedPtr steer_ptr;
+  autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr vehicle_cmd_ptr;
+  autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr turn_signal_ptr;
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_ptr;
+  autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr gear_ptr;
+  std_msgs::msg::Float32::ConstSharedPtr battery_ptr;
+  sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr;
+  autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr;
+  autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr control_mode_ptr;
+  autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr;
+  autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr is_emergency_ptr;
+  autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr;
+  diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr;
+  pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr;
+  std_msgs::msg::Bool::ConstSharedPtr lane_change_available_ptr;
+  std_msgs::msg::Bool::ConstSharedPtr lane_change_ready_ptr;
+  autoware_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr;
+  std_msgs::msg::Bool::ConstSharedPtr obstacle_avoid_ready_ptr;
+  autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr;
 };
 
-template <class T>
-T getParam(const ros::NodeHandle & nh, const std::string & key, const T & default_value)
-{
-  T value;
-  nh.param<T>(key, value, default_value);
-  return value;
-}
-
-template <class T>
-T waitForParam(const ros::NodeHandle & nh, const std::string & key)
-{
-  T value;
-  ros::Rate rate(1.0);
-
-  while (ros::ok()) {
-    const auto result = nh.getParam(key, value);
-    if (result) {
-      return value;
-    }
-
-    ROS_WARN("waiting for parameter `%s` ...", key.c_str());
-    rate.sleep();
-  }
-
-  return {};
-}
-
 double lowpass_filter(const double current_value, const double prev_value, const double gain);
 
 }  // namespace autoware_api