From c6fbce3e76b69e4582bf1a8d3ae6f4bb39777220 Mon Sep 17 00:00:00 2001
From: Timothy Wiley <timothyw@cse.unsw.edu.au>
Date: Wed, 8 Mar 2017 13:56:45 +1100
Subject: [PATCH] crosbot_explore: Basic control client can recieve target
 point or pose

---
 crosbot_explore/CMakeLists.txt                | 20 +++++++---
 .../actionServer/basicClient.hpp              |  6 +++
 .../actionServer/basicClientCommands.hpp      |  2 +
 crosbot_explore/package.xml                   |  1 +
 .../src/actionServer/basicClient.cpp          | 39 +++++++++++++++++++
 5 files changed, 63 insertions(+), 5 deletions(-)

diff --git a/crosbot_explore/CMakeLists.txt b/crosbot_explore/CMakeLists.txt
index 9aea873..f6d2c38 100644
--- a/crosbot_explore/CMakeLists.txt
+++ b/crosbot_explore/CMakeLists.txt
@@ -6,12 +6,17 @@ project(crosbot_explore)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
     roscpp
-    crosbot crosbot_map
-    geometry_msgs nav_msgs
+    geometry_msgs 
+    nav_msgs
     dynamic_reconfigure
     message_generation
-    genmsg actionlib_msgs actionlib
-    )
+    genmsg 
+    actionlib_msgs
+    actionlib
+    crosbot 
+    crosbot_map
+    crosbot_msgs
+)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -77,8 +82,13 @@ catkin_package(
    INCLUDE_DIRS include
    LIBRARIES crosbot_explore
    CATKIN_DEPENDS
-     crosbot crosbot_map roscpp geometry_msgs nav_msgs
+     roscpp 
+     geometry_msgs 
+     nav_msgs
      actionlib_msgs
+     crosbot 
+     crosbot_map
+     crosbot_msgs
 #  DEPENDS system_lib
 )
 
diff --git a/crosbot_explore/include/crosbot_explore/actionServer/basicClient.hpp b/crosbot_explore/include/crosbot_explore/actionServer/basicClient.hpp
index ef65656..4c927f7 100644
--- a/crosbot_explore/include/crosbot_explore/actionServer/basicClient.hpp
+++ b/crosbot_explore/include/crosbot_explore/actionServer/basicClient.hpp
@@ -70,6 +70,12 @@ private:
      */
     void setExploreMode(int mode);
 
+    /**
+     * Get command points
+     */
+    crosbot::Pose3D getCommandPoint(const crosbot_msgs::ControlCommandPtr command);
+    crosbot::Pose3D getCommandPose(const crosbot_msgs::ControlCommandPtr command);
+
     /**
      * Get A* path to origin and set path
      */
diff --git a/crosbot_explore/include/crosbot_explore/actionServer/basicClientCommands.hpp b/crosbot_explore/include/crosbot_explore/actionServer/basicClientCommands.hpp
index 8407d5f..4cd5504 100644
--- a/crosbot_explore/include/crosbot_explore/actionServer/basicClientCommands.hpp
+++ b/crosbot_explore/include/crosbot_explore/actionServer/basicClientCommands.hpp
@@ -11,6 +11,8 @@
 #define DEFAULT_CONTROL_NAMESPACE     "crosbot_explore"
 
 #define COMMAND_GO_TO_ORIGIN          "explore_command_go_to_origin"
+#define COMMAND_GO_TO_POINT           "explore_command_go_to_point"
+#define COMMAND_GO_TO_POSE            "explore_command_go_to_pose"
 #define COMMAND_LEFTWALL_FOLLOW       "explore_command_leftwall_follow"
 #define COMMAND_RIGHTWALL_FOLLOW      "explore_command_rightwall_follow"
 
diff --git a/crosbot_explore/package.xml b/crosbot_explore/package.xml
index e2227ed..0f3b848 100644
--- a/crosbot_explore/package.xml
+++ b/crosbot_explore/package.xml
@@ -35,6 +35,7 @@
     <depend>actionlib_msgs</depend>
     <depend>crosbot</depend>
     <depend>crosbot_map</depend>
+    <depend>crosbot_msgs</depend>
     <depend>dynamic_reconfigure</depend>
     <depend>geometry_msgs</depend>
     <depend>nav_msgs</depend>
diff --git a/crosbot_explore/src/actionServer/basicClient.cpp b/crosbot_explore/src/actionServer/basicClient.cpp
index 4cfcea6..6e10382 100644
--- a/crosbot_explore/src/actionServer/basicClient.cpp
+++ b/crosbot_explore/src/actionServer/basicClient.cpp
@@ -112,6 +112,20 @@ void BasicExplorerClient::callback_receivedCommand(const crosbot_msgs::ControlCo
         setExploreMode(crosbot_explore::ExploreMode::MODE_RIGHT_WALL);
     } else if (command->command == COMMAND_GO_TO_ORIGIN) {
         setAStarPose(crosbot::Pose3D(), false);
+    } else if (command->command == COMMAND_GO_TO_POINT) {
+        crosbot::Pose3D toPose = getCommandPoint(command);
+        if (toPose.isFinite()) {
+            setAStarPose(toPose, false);
+        } else {
+            STATUS_ERROR(crosbotStatus, "%s Go to Point failed - invalid point specified", LOG_START);
+        }
+    } else if (command->command == COMMAND_GO_TO_POSE) {
+        crosbot::Pose3D toPose = getCommandPose(command);
+        if (toPose.isFinite()) {
+            setAStarPose(toPose, true);
+        } else {
+            STATUS_ERROR(crosbotStatus, "%s Go to Pose failed - invalid point specified", LOG_START);
+        }
     }
 }
 
@@ -137,6 +151,31 @@ void BasicExplorerClient::actionserver_feedback(const crosbot_explore::ExploreFe
     STATUS_INFO(crosbotStatus, "Explorer: %s (%s)", feedback->status.c_str(), searchStrategy.c_str());
 }
 
+crosbot::Pose3D BasicExplorerClient::getCommandPoint(const crosbot_msgs::ControlCommandPtr command) {
+    crosbot::Pose3D toPoint(INFINITY, INFINITY, INFINITY);
+
+    if (command->args_doubles.size() >= 3) {
+        toPoint.position.x = command->args_doubles[0];
+        toPoint.position.y = command->args_doubles[1];
+        toPoint.position.z = command->args_doubles[2];
+    }
+
+    return toPoint;
+}
+
+crosbot::Pose3D BasicExplorerClient::getCommandPose(const crosbot_msgs::ControlCommandPtr command) {
+    crosbot::Pose3D toPose = getCommandPoint(command);
+
+    if (command->args_doubles.size() >= 7) {
+        toPose.orientation.x = command->args_doubles[3];
+        toPose.orientation.y = command->args_doubles[4];
+        toPose.orientation.z = command->args_doubles[5];
+        toPose.orientation.w = command->args_doubles[6];
+    }
+
+    return toPose;
+}
+
 void BasicExplorerClient::setExploreMode(int mode) {
     // Ensure action server has started
     bool hasServer = actionClient->waitForServer(ros::Duration(DEFAULT_SERVER_WAIT));
-- 
GitLab