From d61f15f1d9344ada6ddd3aea08ff0f0f3d1e77bf Mon Sep 17 00:00:00 2001
From: Arturo Torres <arturotorres@us.es>
Date: Thu, 28 Jun 2018 13:08:36 +0200
Subject: [PATCH] Set pose and velocity through topics. Fix #33

---
 .../launch/test_server.launch                 |  2 +-
 uav_abstraction_layer/src/ual.cpp             | 19 +++++++++++++------
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/uav_abstraction_layer/launch/test_server.launch b/uav_abstraction_layer/launch/test_server.launch
index e41f3bf..e44b82b 100644
--- a/uav_abstraction_layer/launch/test_server.launch
+++ b/uav_abstraction_layer/launch/test_server.launch
@@ -22,7 +22,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
         <!-- Launch Gazebo simulation -->
         <node pkg="px4_bringup" type="launch_gzworld.py" name="gazebo_world" output="screen"
         args="-physics=ode -world=$(find px4_bringup)/config/empty_light.world">
-            <rosparam param="sim_origin">[37.558542, -5.931074, 7.89]</rosparam>
+            <rosparam param="sim_origin">[37.558542, -5.931074, 7.89]</rosparam><!-- [lat,lon,alt] -->
         </node>
     </group>
 
diff --git a/uav_abstraction_layer/src/ual.cpp b/uav_abstraction_layer/src/ual.cpp
index aaf3fe6..a9627f8 100644
--- a/uav_abstraction_layer/src/ual.cpp
+++ b/uav_abstraction_layer/src/ual.cpp
@@ -65,7 +65,8 @@ void UAL::init() {
             std::string land_srv = ual_ns + "/land";
             std::string go_to_waypoint_srv = ual_ns + "/go_to_waypoint";
             std::string go_to_waypoint_geo_srv = ual_ns + "/go_to_waypoint_geo";
-            std::string set_velocity_srv = ual_ns + "/set_velocity";
+            std::string set_pose_topic = ual_ns + "/set_pose";
+            std::string set_velocity_topic = ual_ns + "/set_velocity";
             std::string recover_from_manual_srv = ual_ns + "/recover_from_manual";
             std::string set_home_srv = ual_ns + "/set_home";
             std::string pose_topic = ual_ns + "/pose";
@@ -97,11 +98,17 @@ void UAL::init() {
                 [this](GoToWaypointGeo::Request &req, GoToWaypointGeo::Response &res) {
                 return this->goToWaypointGeo(req.waypoint, req.blocking);
             });
-            ros::ServiceServer set_velocity_service =
-                nh.advertiseService<SetVelocity::Request, SetVelocity::Response>(
-                set_velocity_srv,
-                [this](SetVelocity::Request &req, SetVelocity::Response &res) {
-                return this->setVelocity(req.velocity);
+            ros::Subscriber set_pose_sub =
+                nh.subscribe<geometry_msgs::PoseStamped>(
+                set_pose_topic, 1,
+                [this](const geometry_msgs::PoseStamped::ConstPtr& _msg) {
+                this->goToWaypoint(*_msg, false);
+            });
+            ros::Subscriber set_velocity_sub =
+                nh.subscribe<geometry_msgs::TwistStamped>(
+                set_velocity_topic, 1,
+                [this](const geometry_msgs::TwistStamped::ConstPtr& _msg) {
+                this->setVelocity(*_msg);
             });
             ros::ServiceServer recover_from_manual_service =
                 nh.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>(