Implemented a SetLocalPosition Service

ros/src/fsds_ros_bridge/CMakeLists.txt

@@ -38,6 +38,13 @@ find_package(catkin REQUIRED COMPONENTS
   fs_msgs
 )
 
+add_service_files(
+  FILES
+  SetLocalPosition.srv
+)
+
+generate_messages()
+
 catkin_package(
   INCLUDE_DIRS include
   # LIBRARIES airsim_ros

ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h

@@ -9,12 +9,14 @@ STRICT_MODE_OFF //todo what does this do?
 #include "statistics.h"
 #include "common/AirSimSettings.hpp"
 #include "common/common_utils/FileSystem.hpp"
+#include "common/VectorMath.hpp"
 #include "ros/ros.h"
 #include "sensors/imu/ImuBase.hpp"
 #include "vehicles/car/api/CarRpcLibClient.hpp"
 #include "yaml-cpp/yaml.h"
 #include <fs_msgs/ControlCommand.h>
 #include <fs_msgs/Reset.h>
+#include <fsds_ros_bridge/SetLocalPosition.h>
 #include <fs_msgs/GoSignal.h>
 #include <fs_msgs/FinishedSignal.h>
 #include <fs_msgs/Track.h>
@@ -137,6 +139,7 @@ class AirsimROSWrapper
 
     /// ROS service callbacks
     bool reset_srv_cb(fs_msgs::Reset::Request& request, fs_msgs::Reset::Response& response);
+    bool set_local_position_srv_cb(fsds_ros_bridge::SetLocalPosition::Request& request, fsds_ros_bridge::SetLocalPosition::Response& response);
 
     // methods which parse setting json ang generate ros pubsubsrv
     void create_ros_pubs_from_settings_json();
@@ -163,6 +166,7 @@ class AirsimROSWrapper
 
 private:
     ros::ServiceServer reset_srvr_;
+    ros::ServiceServer set_local_position_srvr;
 
     std::string vehicle_name;
     CarApiBase::Point2D car_start_pos; // In Unreal coordinates

ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp

@@ -259,6 +259,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
     }
 
     reset_srvr_ = nh_.advertiseService("reset", &AirsimROSWrapper::reset_srv_cb, this);
+    set_local_position_srvr = nh_.advertiseService("set_local_position", &AirsimROSWrapper::set_local_position_srv_cb, this);
 
     initialize_airsim();
 }
@@ -281,6 +282,15 @@ bool AirsimROSWrapper::reset_srv_cb(fs_msgs::Reset::Request& request, fs_msgs::R
     return true; //todo
 }
 
+bool AirsimROSWrapper::set_local_position_srv_cb(fsds_ros_bridge::SetLocalPosition::Request& request, fsds_ros_bridge::SetLocalPosition::Response& response)
+{
+    auto quaternion = msr::airlib::VectorMathf::toQuaternion(0, 0, request.yaw * M_PI / 180);
+    msr::airlib::Vector3r vec3r {request.x, request.y, request.z};
+
+    airsim_client_.simSetVehiclePose(msr::airlib::Pose {vec3r, quaternion}, true, "FSCar");
+    return true;
+}
+
 tf2::Quaternion AirsimROSWrapper::get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const
 {
     return tf2::Quaternion(airlib_quat.x(), airlib_quat.y(), airlib_quat.z(), airlib_quat.w());

ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv

@@ -1,10 +1,9 @@
 #Request : expects position setpoint via x, y, z
 #Request : expects yaw setpoint via yaw (send yaw_valid=true)
-float64 x
-float64 y
-float64 z
-float64 yaw
-string vehicle_name
+float32 x
+float32 y
+float32 z
+float32 yaw # in degree
 ---
 #Response : success=true - (if async=false && if setpoint reached before timeout = 30sec) || (if async=true)
 bool success