Set pose and velocity through topics. Fix #33

grvcTeam · Jun 28, 2018 · d61f15f · d61f15f
1 parent 1f19c7b
commit d61f15f
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 7 deletions.
diff --git 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
@@ -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>(