Feature/test twist gate

ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt

@@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS
         autoware_config_msgs
         tablet_socket_msgs
         autoware_health_checker
+        rostest
+        rosunit
         )
 
 ################################################
@@ -54,6 +56,7 @@ SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
 
 include_directories(
         include
+        include/twist_gate
         ${autoware_config_msgs_INCLUDE_DIRS}
         ${autoware_msgs_INCLUDE_DIRS}
         ${tablet_socket_msgs_INCLUDE_DIRS}
@@ -84,7 +87,7 @@ target_link_libraries(twist_filter ${catkin_LIBRARIES})
 add_dependencies(twist_filter
         ${catkin_EXPORTED_TARGETS})
 
-add_executable(twist_gate nodes/twist_gate/twist_gate.cpp)
+add_executable(twist_gate nodes/twist_gate/twist_gate_node.cpp nodes/twist_gate/twist_gate.cpp)
 target_link_libraries(twist_gate ${catkin_LIBRARIES})
 add_dependencies(twist_gate
         ${catkin_EXPORTED_TARGETS})
@@ -102,3 +105,16 @@ install(DIRECTORY include/${PROJECT_NAME}/
 install(DIRECTORY launch/
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
         PATTERN ".svn" EXCLUDE)
+
+if (CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(test-twist_gate
+      test/test_twist_gate.test
+      test/src/test_twist_gate.cpp
+      nodes/twist_gate/twist_gate.cpp
+    )
+    add_dependencies(test-twist_gate
+      ${catkin_EXPORTED_TARGETS})
+    target_link_libraries(test-twist_gate
+      ${catkin_LIBRARIES})
+endif ()

ros/src/computing/planning/motion/packages/waypoint_follower/include/twist_gate/twist_gate.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 TWIST_GATE_H
+#define TWIST_GATE_H
+
+#include <string>
+#include <iostream>
+#include <map>
+#include <thread>
+#include <memory>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+
+#include "autoware_msgs/ControlCommandStamped.h"
+#include "autoware_msgs/RemoteCmd.h"
+#include "autoware_msgs/VehicleCmd.h"
+
+#include "tablet_socket_msgs/gear_cmd.h"
+#include "tablet_socket_msgs/mode_cmd.h"
+
+//headers in Autowae Health Checker
+#include <autoware_health_checker/node_status_publisher.h>
+
+#define CMD_GEAR_D 1
+#define CMD_GEAR_R 2
+#define CMD_GEAR_B 3
+#define CMD_GEAR_N 4
+#define CMD_GEAR_P 5
+
+class TwistGate
+{
+  using remote_msgs_t = autoware_msgs::RemoteCmd;
+  using vehicle_cmd_msg_t = autoware_msgs::VehicleCmd;
+
+  friend class TwistGateTestClass;
+
+public:
+  TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
+  ~TwistGate();
+
+private:
+  void check_state();
+  void watchdog_timer();
+  void remote_cmd_callback(const remote_msgs_t::ConstPtr& input_msg);
+  void auto_cmd_twist_cmd_callback(const geometry_msgs::TwistStamped::ConstPtr& input_msg);
+  void mode_cmd_callback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg);
+  void gear_cmd_callback(const tablet_socket_msgs::gear_cmd::ConstPtr& input_msg);
+  void accel_cmd_callback(const autoware_msgs::AccelCmd::ConstPtr& input_msg);
+  void steer_cmd_callback(const autoware_msgs::SteerCmd::ConstPtr& input_msg);
+  void brake_cmd_callback(const autoware_msgs::BrakeCmd::ConstPtr& input_msg);
+  void lamp_cmd_callback(const autoware_msgs::LampCmd::ConstPtr& input_msg);
+  void ctrl_cmd_callback(const autoware_msgs::ControlCommandStamped::ConstPtr& input_msg);
+  void state_callback(const std_msgs::StringConstPtr& input_msg);
+
+  bool is_using_decisionmaker();
+  void reset_vehicle_cmd_msg();
+
+  // spinOnce for test
+  void spinOnce(){ ros::spinOnce(); }
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_pub_ptr_;
+  ros::Publisher emergency_stop_pub_;
+  ros::Publisher control_command_pub_;
+  ros::Publisher vehicle_cmd_pub_;
+  ros::Publisher state_cmd_pub_;
+  ros::Subscriber remote_cmd_sub_;
+  std::map<std::string, ros::Subscriber> auto_cmd_sub_stdmap_;
+
+  vehicle_cmd_msg_t twist_gate_msg_;
+  std_msgs::Bool emergency_stop_msg_;
+  ros::Time remote_cmd_time_;
+  ros::Duration timeout_period_;
+
+  std::thread watchdog_timer_thread_;
+  bool is_alive;
+
+  enum class CommandMode
+  {
+    AUTO = 1,
+    REMOTE = 2
+  } command_mode_,
+      previous_command_mode_;
+  std_msgs::String command_mode_topic_;
+
+  bool is_state_drive_ = true;
+  // still send is true
+  bool send_emergency_cmd = false;
+};
+
+#endif  // TWIST_GATE_H

ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp

@@ -28,88 +28,9 @@
  *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <chrono>
-#include <iostream>
-#include <map>
-#include <thread>
-#include <memory>
-
-#include <geometry_msgs/TwistStamped.h>
-#include <ros/ros.h>
-#include <std_msgs/Bool.h>
-#include <std_msgs/Int32.h>
-#include <std_msgs/String.h>
-
-#include "autoware_msgs/ControlCommandStamped.h"
-#include "autoware_msgs/RemoteCmd.h"
-#include "autoware_msgs/VehicleCmd.h"
-
-#define CMD_GEAR_D 1
-#define CMD_GEAR_R 2
-#define CMD_GEAR_B 3
-#define CMD_GEAR_N 4
-#define CMD_GEAR_P 5
-#include "autoware_msgs/ControlCommandStamped.h"
-#include "tablet_socket_msgs/gear_cmd.h"
-#include "tablet_socket_msgs/mode_cmd.h"
-
-//headers in Autowae Health Checker
-#include <autoware_health_checker/node_status_publisher.h>
-
-class TwistGate
-{
-  using remote_msgs_t = autoware_msgs::RemoteCmd;
-  using vehicle_cmd_msg_t = autoware_msgs::VehicleCmd;
-
-public:
-  TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
-  ~TwistGate();
-
-private:
-  void check_state();
-  void watchdog_timer();
-  void remote_cmd_callback(const remote_msgs_t::ConstPtr& input_msg);
-  void auto_cmd_twist_cmd_callback(const geometry_msgs::TwistStamped::ConstPtr& input_msg);
-  void mode_cmd_callback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg);
-  void gear_cmd_callback(const tablet_socket_msgs::gear_cmd::ConstPtr& input_msg);
-  void accel_cmd_callback(const autoware_msgs::AccelCmd::ConstPtr& input_msg);
-  void steer_cmd_callback(const autoware_msgs::SteerCmd::ConstPtr& input_msg);
-  void brake_cmd_callback(const autoware_msgs::BrakeCmd::ConstPtr& input_msg);
-  void lamp_cmd_callback(const autoware_msgs::LampCmd::ConstPtr& input_msg);
-  void ctrl_cmd_callback(const autoware_msgs::ControlCommandStamped::ConstPtr& input_msg);
-  void state_callback(const std_msgs::StringConstPtr& input_msg);
-
-  bool is_using_decisionmaker();
-  void reset_vehicle_cmd_msg();
-
-  ros::NodeHandle nh_;
-  ros::NodeHandle private_nh_;
-  std::shared_ptr<autoware_health_checker::NodeStatusPublisher> node_status_pub_ptr_;
-  ros::Publisher emergency_stop_pub_;
-  ros::Publisher control_command_pub_;
-  ros::Publisher vehicle_cmd_pub_;
-  ros::Publisher state_cmd_pub_;
-  ros::Subscriber remote_cmd_sub_;
-  std::map<std::string, ros::Subscriber> auto_cmd_sub_stdmap_;
-
-  vehicle_cmd_msg_t twist_gate_msg_;
-  std_msgs::Bool emergency_stop_msg_;
-  ros::Time remote_cmd_time_;
-  ros::Duration timeout_period_;
-
-  std::thread watchdog_timer_thread_;
-  enum class CommandMode
-  {
-    AUTO = 1,
-    REMOTE = 2
-  } command_mode_,
-      previous_command_mode_;
-  std_msgs::String command_mode_topic_;
+#include "twist_gate.h"
 
-  bool is_state_drive_ = true;
-  // still send is true
-  bool send_emergency_cmd = false;
-};
+#include <chrono>
 
 TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh)
   : nh_(nh)
@@ -143,11 +64,13 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n
 
   remote_cmd_time_ = ros::Time::now();
   watchdog_timer_thread_ = std::thread(&TwistGate::watchdog_timer, this);
-  watchdog_timer_thread_.detach();
+  is_alive = true;
 }
 
 TwistGate::~TwistGate()
 {
+  is_alive = false;
+  watchdog_timer_thread_.join();
 }
 
 void TwistGate::reset_vehicle_cmd_msg()
@@ -193,7 +116,7 @@ void TwistGate::check_state()
 
 void TwistGate::watchdog_timer()
 {
-  while (1)
+  while (is_alive)
   {
     ros::Time now_time = ros::Time::now();
     bool emergency_flag = false;
@@ -422,15 +345,3 @@ void TwistGate::state_callback(const std_msgs::StringConstPtr& input_msg)
     }
   }
 }
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "twist_gate");
-  ros::NodeHandle nh;
-  ros::NodeHandle private_nh("~");
-
-  TwistGate twist_gate(nh, private_nh);
-
-  ros::spin();
-  return 0;
-}

ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate_node.cpp

@@ -0,0 +1,33 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+// ROS Includes
+#include <ros/ros.h>
+
+// User defined includes
+#include "twist_gate.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "twist_gate");
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  TwistGate twist_gate(nh, private_nh);
+
+  ros::spin();
+  return 0;
+}

ros/src/computing/planning/motion/packages/waypoint_follower/package.xml

@@ -19,6 +19,8 @@
     <build_depend>sensor_msgs</build_depend>
     <build_depend>tablet_socket_msgs</build_depend>
     <build_depend>autoware_health_checker</build_depend>
+    <build_depend>rostest</build_depend>
+    <build_depend>rosunit</build_depend>
 
     <run_depend>roscpp</run_depend>
     <run_depend>std_msgs</run_depend>