From 4e4b65f8967f080b7bc9dc6991f05792ede5b8d1 Mon Sep 17 00:00:00 2001
From: sadowsky <dsadowsky@ufl.edu>
Date: Tue, 23 Jul 2019 16:01:24 -0400
Subject: [PATCH] DIAGNOSTIC: add movement test

---
 utils/sub8_diagnostics/CMakeLists.txt    | 53 ++++++++++++++++-
 utils/sub8_diagnostics/package.xml       | 27 ++++++++-
 utils/sub8_diagnostics/src/move_test.cpp | 72 ++++++++++++++++++++++++
 3 files changed, 149 insertions(+), 3 deletions(-)
 create mode 100755 utils/sub8_diagnostics/src/move_test.cpp

diff --git a/utils/sub8_diagnostics/CMakeLists.txt b/utils/sub8_diagnostics/CMakeLists.txt
index 5c1592d0..a03694f1 100644
--- a/utils/sub8_diagnostics/CMakeLists.txt
+++ b/utils/sub8_diagnostics/CMakeLists.txt
@@ -1,18 +1,67 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(sub8_diagnostics)
 
-find_package(catkin REQUIRED COMPONENTS
+find_package(catkin
+ REQUIRED COMPONENTS
+  roscpp
   rospy
   mil_tools
+  message_runtime
+  sub8_msgs
+  mil_msgs
+  actionlib
+  message_generation
+  geometry_msgs
+  sensor_msgs
+  interactive_markers
+  std_msgs
+  actionlib_msgs
+  mil_tools
+)
+
+catkin_python_setup()
+
+generate_messages(
+    DEPENDENCIES std_msgs actionlib_msgs geometry_msgs sensor_msgs
 )
 
+
 # Any tools used to diagnose the core functionality of the sub should be put here
 catkin_package(
  INCLUDE_DIRS
  LIBRARIES
  CATKIN_DEPENDS
+  actionlib
+  message_runtime
+  mil_msgs
   mil_tools
+  roscpp
+  sub8_msgs
+  message_generation
+  message_runtime
+  geometry_msgs
+  std_msgs
+  actionlib_msgs
  DEPENDS
+  mil_msgs
 )
 
-catkin_python_setup()
+
+
+
+
+
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+
+add_executable(move_test src/move_test.cpp)
+
+target_link_libraries(
+  move_test
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  move_test
+  ${mil_msgs_EXPORTED_TARGETS}
+)
diff --git a/utils/sub8_diagnostics/package.xml b/utils/sub8_diagnostics/package.xml
index 1822d2b1..2811e156 100644
--- a/utils/sub8_diagnostics/package.xml
+++ b/utils/sub8_diagnostics/package.xml
@@ -9,8 +9,33 @@
   <license>MIT</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_runtime</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <build_depend>rospy</build_depend>
-  <run_depend>rospy</run_depend>
   <build_depend>mil_tools</build_depend>
+  <build_depend>mil_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sub8_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+
+  <run_depend>message_runtime</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>rospy</run_depend>
   <run_depend>mil_tools</run_depend>
+  <run_depend>mil_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sub8_msgs</run_depend>
+  <run_depend>actionlib</run_depend>
+
 </package>
diff --git a/utils/sub8_diagnostics/src/move_test.cpp b/utils/sub8_diagnostics/src/move_test.cpp
new file mode 100755
index 00000000..2f3d7baa
--- /dev/null
+++ b/utils/sub8_diagnostics/src/move_test.cpp
@@ -0,0 +1,72 @@
+#include <ros/ros.h>
+#include <mil_msgs/MoveToActionResult.h>
+#include <mil_msgs/MoveToAction.h>
+#include <mil_msgs/MoveToActionGoal.h>
+#include <mil_msgs/MoveToGoal.h>
+#include <actionlib/client/simple_action_client.h>
+#include <stdio.h>
+#include <string>
+#include <iostream>
+
+using namespace std;
+
+// declares MoveToAction action client
+typedef actionlib::SimpleActionClient<mil_msgs::MoveToAction> MoveToActionClient;
+
+// first arg = forward command, second arg = up command
+int main(int argc, char** argv){
+  ros::init(argc, argv, "move_test");
+
+  // not sure what the server is called, figure out what the server is actually called
+  MoveToActionClient ac("/moveto", true);
+
+    while(!ac.waitForServer(ros::Duration(5.0))){
+      ROS_INFO("Waiting for the moveto server to come up!");
+    }
+
+    string x = "";
+    string y = "";
+
+    if (argc < 2) {
+      cout << "Error: No command found.";
+      return 0;
+    }
+
+    // declares goal for the sub
+    mil_msgs::MoveToGoal goal;
+    x = string(argv[1]);
+
+    goal.header.frame_id = "base_link";
+    goal.header.stamp = ros::Time::now();
+
+    // sets goal to x meter forward
+    goal.posetwist.pose.position.x = atoi(argv[1]);
+    goal.posetwist.pose.orientation.w = atoi(argv[1]);
+
+    // sets goal to y meters upward if an arg is found
+    if(argc == 3) {
+      goal.posetwist.pose.position.y = atoi(argv[2]);
+      x = string(argv[1]);
+      y = string(argv[2]);
+    }
+
+    ROS_INFO("Sending goal");
+
+    ac.sendGoal(goal);
+    ac.waitForResult();
+
+    // checks current robot state vs. goal state
+    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+    cout << "The sub moved " + x + " meter(s) forward" << endl;
+    if(argc == 3) {
+      cout << "The sub moved " + y + " meter(s) up";
+    }
+  }
+    else {
+    cout << "The sub failed to move forward " + x + " meter(s)" << endl;
+    if(argc == 3){
+      "The sub failed to move up " + y + " meters(s)";
+    }
+}
+    return 0;
+}