Port vehicle_cmd_gate to ROS2

common/msgs/autoware_control_msgs/CMakeLists.txt

@@ -13,6 +13,8 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/EmergencyMode.msg"
+  "msg/EngageMode.msg"
   "msg/GateMode.msg"
   "msg/ControlCommand.msg"
   "msg/ControlCommandStamped.msg"

common/msgs/autoware_control_msgs/msg/EmergencyMode.msg

@@ -0,0 +1 @@
+bool is_emergency

common/msgs/autoware_control_msgs/msg/EngageMode.msg

@@ -0,0 +1 @@
+bool is_engaged

control/vehicle_cmd_gate/CMakeLists.txt

@@ -1,48 +1,45 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(vehicle_cmd_gate)
 
-add_compile_options(-std=c++14)
-
-find_package(
-  catkin REQUIRED COMPONENTS
-    autoware_control_msgs
-    autoware_vehicle_msgs
-    geometry_msgs
-    roscpp
-    rostest
-    rosunit
-    roslint
-    std_msgs
-)
+### Compile options
+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)
+  add_compile_options(-Wno-unused-parameter)
+endif()
 
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS
-    autoware_control_msgs
-    autoware_vehicle_msgs
-    geometry_msgs
-    std_msgs
-)
+### Dependencies
+find_package(ament_cmake REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
 
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
+# Add path of include dir
+include_directories(include)
 
+# Generate exe file
 add_executable(vehicle_cmd_gate src/vehicle_cmd_gate_node.cpp src/vehicle_cmd_gate.cpp src/vehicle_cmd_filter.cpp)
-target_link_libraries(vehicle_cmd_gate ${catkin_LIBRARIES})
-add_dependencies(vehicle_cmd_gate ${catkin_EXPORTED_TARGETS})
+
+set(VEHICLE_CMD_GATE_DEPENDENCIES
+	autoware_control_msgs
+	autoware_vehicle_msgs
+	rclcpp
+)
+ament_target_dependencies(vehicle_cmd_gate ${VEHICLE_CMD_GATE_DEPENDENCIES})
+ament_export_dependencies(${VEHICLE_CMD_GATE_DEPENDENCIES})
 
 install(
   TARGETS vehicle_cmd_gate
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 install(
   DIRECTORY
     launch
     config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share/${PROJECT_NAME}
 )
+
+ament_package()

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    use_emergency_handling: false
+    vel_lim: 25.0
+    lon_acc_lim: 5.0
+    lon_jerk_lim: 5.0
+    lat_acc_lim: 5.0
+    lat_jerk_lim: 5.0

control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.h

@@ -17,10 +17,7 @@
 #ifndef VEHICLE_CMD_GATE_VEHICLE_CMD_FILTER_H
 #define VEHICLE_CMD_GATE_VEHICLE_CMD_FILTER_H
 
-#include <ros/ros.h>
-#include <std_msgs/Bool.h>
-
-#include "autoware_control_msgs/ControlCommandStamped.h"
+#include <autoware_control_msgs/msg/control_command_stamped.hpp>
 
 class VehicleCmdFilter
 {
@@ -34,13 +31,15 @@ class VehicleCmdFilter
   void setLonJerkLim(double v) { lon_jerk_lim_ = v; };
   void setLatAccLim(double v) { lat_acc_lim_ = v; };
   void setLatJerkLim(double v) { lat_jerk_lim_ = v; };
-  void setPrevCmd(const autoware_control_msgs::ControlCommand & v) { prev_cmd_ = v; };
+  void setPrevCmd(const autoware_control_msgs::msg::ControlCommand & v) { prev_cmd_ = v; };
 
-  void limitLongitudinalWithVel(autoware_control_msgs::ControlCommand & input);
-  void limitLongitudinalWithAcc(const double dt, autoware_control_msgs::ControlCommand & input);
-  void limitLongitudinalWithJerk(const double dt, autoware_control_msgs::ControlCommand & input);
-  void limitLateralWithLatAcc(const double dt, autoware_control_msgs::ControlCommand & input);
-  void limitLateralWithLatJerk(const double dt, autoware_control_msgs::ControlCommand & input);
+  void limitLongitudinalWithVel(autoware_control_msgs::msg::ControlCommand & input);
+  void limitLongitudinalWithAcc(
+    const double dt, autoware_control_msgs::msg::ControlCommand & input);
+  void limitLongitudinalWithJerk(
+    const double dt, autoware_control_msgs::msg::ControlCommand & input);
+  void limitLateralWithLatAcc(const double dt, autoware_control_msgs::msg::ControlCommand & input);
+  void limitLateralWithLatJerk(const double dt, autoware_control_msgs::msg::ControlCommand & input);
 
 private:
   double wheel_base_;
@@ -49,9 +48,9 @@ class VehicleCmdFilter
   double lon_jerk_lim_;
   double lat_acc_lim_;
   double lat_jerk_lim_;
-  autoware_control_msgs::ControlCommand prev_cmd_;
+  autoware_control_msgs::msg::ControlCommand prev_cmd_;
 
-  double calcLatAcc(const autoware_control_msgs::ControlCommand & cmd);
+  double calcLatAcc(const autoware_control_msgs::msg::ControlCommand & cmd);
   double limitDiff(const double curr, const double prev, const double diff_lim);
 };
 

control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.h

@@ -19,100 +19,104 @@
 
 #include <memory>
 
-#include <ros/ros.h>
-#include <std_msgs/Bool.h>
+#include <rclcpp/rclcpp.hpp>
 
-#include "autoware_control_msgs/ControlCommandStamped.h"
-#include "autoware_control_msgs/GateMode.h"
-#include "autoware_vehicle_msgs/ShiftStamped.h"
-#include "autoware_vehicle_msgs/TurnSignal.h"
-#include "autoware_vehicle_msgs/VehicleCommand.h"
-#include "vehicle_cmd_gate/vehicle_cmd_filter.h"
+#include <autoware_control_msgs/msg/control_command_stamped.hpp>
+#include <autoware_control_msgs/msg/emergency_mode.hpp>
+#include <autoware_control_msgs/msg/engage_mode.hpp>
+#include <autoware_control_msgs/msg/gate_mode.hpp>
+#include <autoware_vehicle_msgs/msg/shift_stamped.hpp>
+#include <autoware_vehicle_msgs/msg/turn_signal.hpp>
+#include <autoware_vehicle_msgs/msg/vehicle_command.hpp>
+
+#include <vehicle_cmd_gate/vehicle_cmd_filter.h>
 
 struct Commands
 {
-  autoware_control_msgs::ControlCommandStamped control;
-  autoware_vehicle_msgs::TurnSignal turn_signal;
-  autoware_vehicle_msgs::ShiftStamped shift;
+  autoware_control_msgs::msg::ControlCommandStamped control;
+  autoware_vehicle_msgs::msg::TurnSignal turn_signal;
+  autoware_vehicle_msgs::msg::ShiftStamped shift;
 };
 
-class VehicleCmdGate
+class VehicleCmdGate : public rclcpp::Node
 {
 public:
   VehicleCmdGate();
 
 private:
-  // NodeHandle
-  ros::NodeHandle nh_;
-  ros::NodeHandle pnh_;
-
   // Publisher
-  ros::Publisher vehicle_cmd_pub_;
-  ros::Publisher control_cmd_pub_;
-  ros::Publisher shift_cmd_pub_;
-  ros::Publisher turn_signal_cmd_pub_;
-  ros::Publisher gate_mode_pub_;
+  rclcpp::Publisher<autoware_vehicle_msgs::msg::VehicleCommand>::SharedPtr vehicle_cmd_pub_;
+  rclcpp::Publisher<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr control_cmd_pub_;
+  rclcpp::Publisher<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr shift_cmd_pub_;
+  rclcpp::Publisher<autoware_vehicle_msgs::msg::TurnSignal>::SharedPtr turn_signal_cmd_pub_;
+  rclcpp::Publisher<autoware_control_msgs::msg::GateMode>::SharedPtr gate_mode_pub_;
 
-  // Subscriber
-  ros::Subscriber engage_sub_;
-  ros::Subscriber emergency_sub_;
-  ros::Subscriber gate_mode_sub_;
+  // Subscription
+  rclcpp::Subscription<autoware_control_msgs::msg::EngageMode>::SharedPtr engage_sub_;
+  rclcpp::Subscription<autoware_control_msgs::msg::EmergencyMode>::SharedPtr emergency_sub_;
+  rclcpp::Subscription<autoware_control_msgs::msg::GateMode>::SharedPtr gate_mode_sub_;
 
-  void onGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg);
-  void onEngage(const std_msgs::Bool::ConstPtr msg);
-  void onEmergency(const std_msgs::Bool::ConstPtr msg);
+  void onGateMode(autoware_control_msgs::msg::GateMode::ConstSharedPtr msg);
+  void onEngage(autoware_control_msgs::msg::EngageMode::ConstSharedPtr msg);
+  void onEmergency(autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg);
 
   bool is_engaged_;
   bool is_emergency_;
-  autoware_control_msgs::GateMode current_gate_mode_;
+  autoware_control_msgs::msg::GateMode current_gate_mode_;
 
-  // Subscriber for auto
+  // Subscription for auto
   Commands auto_commands_;
-  ros::Subscriber auto_control_cmd_sub_;
-  ros::Subscriber auto_turn_signal_cmd_sub_;
-  ros::Subscriber auto_shift_cmd_sub_;
-  void onAutoCtrlCmd(const autoware_control_msgs::ControlCommandStamped::ConstPtr & msg);
-  void onAutoTurnSignalCmd(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg);
-  void onAutoShiftCmd(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg);
-
-  // Subscriber for remote
+  rclcpp::Subscription<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr
+    auto_control_cmd_sub_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::TurnSignal>::SharedPtr auto_turn_signal_cmd_sub_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr auto_shift_cmd_sub_;
+  void onAutoCtrlCmd(autoware_control_msgs::msg::ControlCommandStamped::ConstSharedPtr msg);
+  void onAutoTurnSignalCmd(autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg);
+  void onAutoShiftCmd(autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg);
+
+  // Subscription for remote
   Commands remote_commands_;
-  ros::Subscriber remote_control_cmd_sub_;
-  ros::Subscriber remote_turn_signal_cmd_sub_;
-  ros::Subscriber remote_shift_cmd_sub_;
-  void onRemoteCtrlCmd(const autoware_control_msgs::ControlCommandStamped::ConstPtr & msg);
-  void onRemoteTurnSignalCmd(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg);
-  void onRemoteShiftCmd(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg);
-
-  // Subscriber for emergency
+  rclcpp::Subscription<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr
+    remote_control_cmd_sub_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::TurnSignal>::SharedPtr
+    remote_turn_signal_cmd_sub_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr remote_shift_cmd_sub_;
+  void onRemoteCtrlCmd(autoware_control_msgs::msg::ControlCommandStamped::ConstSharedPtr msg);
+  void onRemoteTurnSignalCmd(autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg);
+  void onRemoteShiftCmd(autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg);
+
+  // Subscription for emergency
   Commands emergency_commands_;
-  ros::Subscriber emergency_control_cmd_sub_;
-  ros::Subscriber emergency_turn_signal_cmd_sub_;
-  ros::Subscriber emergency_shift_cmd_sub_;
-  void onEmergencyCtrlCmd(const autoware_control_msgs::ControlCommandStamped::ConstPtr & msg);
-  void onEmergencyTurnSignalCmd(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg);
-  void onEmergencyShiftCmd(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg);
+  rclcpp::Subscription<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr
+    emergency_control_cmd_sub_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::TurnSignal>::SharedPtr
+    emergency_turn_signal_cmd_sub_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr
+    emergency_shift_cmd_sub_;
+  void onEmergencyCtrlCmd(autoware_control_msgs::msg::ControlCommandStamped::ConstSharedPtr msg);
+  void onEmergencyTurnSignalCmd(autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg);
+  void onEmergencyShiftCmd(autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg);
 
   // Parameter
-  double update_rate_;
+  double update_period_;
   bool use_emergency_handling_;
 
   // Timer / Event
-  ros::Timer timer_;
+  rclcpp::TimerBase::SharedPtr timer_;
 
-  void onTimer(const ros::TimerEvent & event);
+  void onTimer();
   void publishControlCommands(const Commands & input_msg);
 
   // Algorithm
-  autoware_control_msgs::ControlCommand prev_control_cmd_;
-  autoware_control_msgs::ControlCommand createStopControlCmd() const;
+  autoware_control_msgs::msg::ControlCommand prev_control_cmd_;
+  autoware_control_msgs::msg::ControlCommand createStopControlCmd() const;
 
-  std::shared_ptr<ros::Time> prev_time_;
+  std::shared_ptr<rclcpp::Time> prev_time_;
   double getDt();
 
   VehicleCmdFilter filter_;
-  autoware_control_msgs::ControlCommand filterControlCommand(
-    const autoware_control_msgs::ControlCommand & msg);
+  autoware_control_msgs::msg::ControlCommand filterControlCommand(
+    const autoware_control_msgs::msg::ControlCommand & msg);
 };
 
 #endif  // VEHICLE_CMD_GATE_VEHICLE_CMD_GATE_H

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -0,0 +1,29 @@
+<launch>
+
+  <node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate" name="vehicle_cmd_gate" output="screen">
+    <remap from="input/engage" to="/autoware/engage"/>
+    <remap from="input/emergency" to="/system/emergency/is_emergency"/>
+    <remap from="input/gate_mode" to="/remote/gate_mode_cmd"/>
+
+    <remap from="input/auto/control_cmd" to="trajectory_follower/control_cmd"/>
+    <remap from="input/auto/turn_signal_cmd" to="/planning/turn_signal_decider/turn_signal_cmd"/>
+    <remap from="input/auto/shift_cmd" to="/control/shift_decider/shift_cmd"/>
+
+    <remap from="input/remote/control_cmd" to="/remote/control_cmd"/>
+    <remap from="input/remote/turn_signal_cmd" to="/remote/turn_signal_cmd"/>
+    <remap from="input/remote/shift_cmd" to="/remote/shift_cmd"/>
+
+    <remap from="input/emergency/control_cmd" to="/system/emergency/control_cmd"/>
+    <remap from="input/emergency/turn_signal_cmd" to="/system/emergency/turn_signal_cmd"/>
+    <remap from="input/emergency/shift_cmd" to="/system/emergency/shift_cmd"/>
+
+    <remap from="output/vehicle_cmd" to="vehicle_cmd"/>
+    <remap from="output/control_cmd" to="/control/control_cmd"/>
+    <remap from="output/shift_cmd" to="/control/shift_cmd"/>
+    <remap from="output/turn_signal_cmd" to="/control/turn_signal_cmd"/>
+    <remap from="output/gate_mode" to="/control/current_gate_mode"/>
+
+    <param from="$(find-pkg-share vehicle_cmd_gate)/config/vehicle_cmd_gate.param.yaml" />
+  </node>
+
+</launch>