Add C++ library for mil_missions (#1204)

* Add C++ library for

* clang-format

mil_common/mil_missions/CMakeLists.txt

@@ -1,20 +1,48 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(mil_missions)
-find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -pedantic -Wall -std=c++11")
+
+find_package(catkin REQUIRED rostest genmsg actionlib_msgs roscpp actionlib)
 catkin_python_setup()
 add_action_files(
   FILES
     DoMission.action
 )
+
 generate_messages(
   DEPENDENCIES
     std_msgs
     actionlib_msgs
 )
-catkin_package()
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    mil_missions
+  CATKIN_DEPENDS
+    actionlib_msgs
+    roscpp
+)
+
+include_directories(
+    include
+  SYSTEM
+    ${catkin_INCLUDE_DIRS}
+)
 
 if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
     add_rostest(test/import_mil_missions_examples.test)
-    add_rostest(test/run_mission.test)
+    add_rostest(test/run_mission_py.test)
+    add_rostest_gtest(run_test_mission_cpp
+                      test/run_mission_cpp.test
+                      test/test_run_mission.cpp
+                      src/client.cpp)
+    target_link_libraries(run_test_mission_cpp ${catkin_LIBRARIES})
 endif()
+
+add_library(mil_missions
+  src/client.cpp
+)
+target_include_directories(mil_missions PUBLIC include)
+target_link_libraries(mil_missions ${catkin_LIBRARIES})

mil_common/mil_missions/include/client.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/simple_client_goal_state.h>
+#include <mil_missions/DoMissionAction.h>
+#include <ros/ros.h>
+
+#include <string>
+#include <vector>
+
+namespace mil_missions
+{
+class MissionClient
+{
+public:
+  MissionClient(ros::NodeHandle &nh);
+
+  // About missions
+  std::vector<std::string> available_missions();
+
+  // Execute missions
+  bool wait_for_server(const ros::Duration timeout = ros::Duration(5.0));
+  bool wait_for_result(const ros::Duration timeout = ros::Duration(0));
+  DoMissionResult::ConstPtr get_result();
+  actionlib::SimpleClientGoalState get_state();
+  void run_mission(std::string mission_name, std::string parameters = "");
+  void cancel_mission();
+
+private:
+  actionlib::SimpleActionClient<mil_missions::DoMissionAction> ac;
+  ros::NodeHandle _nh;
+};
+}  // namespace mil_missions

mil_common/mil_missions/include/mil_missions/client.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/simple_client_goal_state.h>
+#include <mil_missions/DoMissionAction.h>
+#include <ros/ros.h>
+
+#include <string>
+#include <vector>
+
+namespace mil_missions
+{
+class MissionClient
+{
+public:
+  MissionClient(ros::NodeHandle &nh);
+
+  // About missions
+  std::vector<std::string> available_missions();
+
+  // Execute missions
+  bool wait_for_server(const ros::Duration timeout = ros::Duration(5.0));
+  bool wait_for_result(const ros::Duration timeout = ros::Duration(0));
+  DoMissionResult::ConstPtr get_result();
+  actionlib::SimpleClientGoalState get_state();
+  void run_mission(std::string mission_name, std::string parameters = "");
+  void cancel_mission();
+
+private:
+  actionlib::SimpleActionClient<mil_missions::DoMissionAction> ac;
+  ros::NodeHandle _nh;
+};
+}  // namespace mil_missions
+// #include "../../src/client.cpp"

mil_common/mil_missions/package.xml

@@ -8,7 +8,9 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>actionlib_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
   <run_depend>actionlib</run_depend>
+  <run_depend>roscpp</run_depend>
   <run_depend>actionlib_msgs</run_depend>
   <run_depend>python_qt_binding</run_depend>
   <run_depend>qt_gui.plugin</run_depend>

mil_common/mil_missions/src/client.cpp

@@ -0,0 +1,59 @@
+#include <mil_missions/client.h>
+
+namespace mil_missions
+{
+MissionClient::MissionClient(ros::NodeHandle &nh) : ac("/mission", true), _nh(nh)
+{
+}
+
+std::vector<std::string> MissionClient::available_missions()
+{
+  std::vector<std::string> missions;
+  if (!_nh.getParam("/available_missions", missions))
+  {
+    ROS_ERROR("No available missions found");
+    return {};
+  }
+  return missions;
+}
+
+void MissionClient::run_mission(std::string mission_name, std::string parameters)
+{
+  if (!ac.waitForServer(ros::Duration(5.0)))
+  {
+    ROS_ERROR("Could not connect to mission server to run mission");
+    return;
+  }
+
+  mil_missions::DoMissionGoal goal;
+  goal.mission = mission_name;
+  goal.parameters = parameters;
+
+  ac.sendGoal(goal);
+}
+
+void MissionClient::cancel_mission()
+{
+  ac.cancelAllGoals();
+}
+
+bool MissionClient::wait_for_server(ros::Duration timeout)
+{
+  return ac.waitForServer(timeout);
+}
+
+bool MissionClient::wait_for_result(ros::Duration timeout)
+{
+  return ac.waitForResult(timeout);
+}
+
+DoMissionResult::ConstPtr MissionClient::get_result()
+{
+  return ac.getResult();
+}
+
+actionlib::SimpleClientGoalState MissionClient::get_state()
+{
+  return ac.getState();
+}
+}  // namespace mil_missions

mil_common/mil_missions/test/run_mission_cpp.test

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="mil_missions" type="mission_server" name="mission_server">
+    <param name="missions_module" value="mil_missions_examples" />
+    <param name="base_mission" value="ExampleBaseMission" />
+  </node>
+  <test pkg="mil_missions" type="run_test_mission_cpp" test-name="run_test_mission_cpp" />
+</launch>

mil_common/mil_missions/test/run_mission.test → mil_common/mil_missions/test/run_mission_py.test

@@ -3,5 +3,5 @@
     <param name="missions_module" value="mil_missions_examples" />
     <param name="base_mission" value="ExampleBaseMission" />
   </node>
-  <test pkg="mil_missions" type="test_run_mission.py" test-name="run_missions" />
+  <test pkg="mil_missions" type="test_run_mission.py" test-name="run_missions_py" />
 </launch>

mil_common/mil_missions/test/test_run_mission.cpp

@@ -0,0 +1,72 @@
+#include <actionlib/client/simple_action_client.h>
+#include <gtest/gtest.h>
+#include <mil_missions/DoMissionResult.h>
+#include <mil_missions/client.h>
+#include <ros/ros.h>
+
+#include <iostream>
+
+using namespace mil_missions;
+
+class MissionClientTest : public ::testing::Test
+{
+protected:
+  MissionClientTest() : client(nh)
+  {
+  }
+
+  ros::NodeHandle nh;
+  mil_missions::MissionClient client;
+};
+
+TEST_F(MissionClientTest, RunMission)
+{
+  EXPECT_TRUE(client.wait_for_server());
+
+  // Test running a mission
+  client.run_mission("PrintAndWait");
+  EXPECT_TRUE(client.wait_for_result(ros::Duration(5.0)));
+  mil_missions::DoMissionResult::ConstPtr result = client.get_result();
+  actionlib::SimpleClientGoalState state = client.get_state();
+  EXPECT_EQ(state, actionlib::SimpleClientGoalState::SUCCEEDED);
+  EXPECT_TRUE(result->success);
+  EXPECT_EQ(result->parameters, "");
+  EXPECT_EQ(result->result, "The darkness isn't so scary");
+}
+
+TEST_F(MissionClientTest, FailingMission)
+{
+  EXPECT_TRUE(client.wait_for_server());
+
+  // Test running a mission
+  client.run_mission("FailingMission");
+  EXPECT_TRUE(client.wait_for_result(ros::Duration(5.0)));
+  mil_missions::DoMissionResult::ConstPtr result = client.get_result();
+  actionlib::SimpleClientGoalState state = client.get_state();
+  EXPECT_EQ(state, actionlib::SimpleClientGoalState::ABORTED);
+  EXPECT_FALSE(result->success);
+  EXPECT_TRUE(!result->parameters.empty());
+  EXPECT_EQ(result->result, "Exception occurred in FailingMission mission: ValueError: This is an example error!");
+}
+
+TEST_F(MissionClientTest, CancelledTest)
+{
+  EXPECT_TRUE(client.wait_for_server());
+
+  // Test running a mission
+  client.run_mission("CancelledMission");
+  EXPECT_TRUE(client.wait_for_result(ros::Duration(5.0)));
+  mil_missions::DoMissionResult::ConstPtr result = client.get_result();
+  actionlib::SimpleClientGoalState state = client.get_state();
+  EXPECT_EQ(state, actionlib::SimpleClientGoalState::ABORTED);
+  EXPECT_FALSE(result->success);
+  EXPECT_TRUE(result->parameters.empty());
+  EXPECT_EQ(result->result, "mission cancelled");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "run_mission_test_cpp");
+  return RUN_ALL_TESTS();
+}

mil_common/mil_missions/test/test_run_mission.py

@@ -49,5 +49,5 @@ def test_cancelled_mission(self):
 if __name__ == "__main__":
     import rostest
 
-    rospy.init_node("run_mission_test")
-    rostest.rosrun("mil_missions", "run_mission", RunMissionTest)
+    rospy.init_node("run_mission_test_py")
+    rostest.rosrun("mil_missions", "run_missions_py", RunMissionTest)