From eb50b7ff293a03c847ce13bf62cc56bdf325de83 Mon Sep 17 00:00:00 2001 From: MarvinKlemp Date: Mon, 4 Jan 2021 22:41:57 +0100 Subject: [PATCH 1/2] Implemented basic set_local_position service --- ros/src/fsds_ros_bridge/CMakeLists.txt | 1 + ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h | 4 ++++ ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp | 10 ++++++++++ ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv | 8 ++++---- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/ros/src/fsds_ros_bridge/CMakeLists.txt b/ros/src/fsds_ros_bridge/CMakeLists.txt index d76d8cb9..6437c2f9 100644 --- a/ros/src/fsds_ros_bridge/CMakeLists.txt +++ b/ros/src/fsds_ros_bridge/CMakeLists.txt @@ -41,6 +41,7 @@ find_package(catkin REQUIRED COMPONENTS add_service_files( FILES Reset.srv + SetLocalPosition.srv ) generate_messages( diff --git a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h index 702f1c23..485f65f2 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/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 #include +#include #include #include #include @@ -137,6 +139,7 @@ class AirsimROSWrapper /// ROS service callbacks bool reset_srv_cb(fsds_ros_bridge::Reset::Request& request, fsds_ros_bridge::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 diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp index b2ac05fd..eb3851b3 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/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(fsds_ros_bridge::Reset::Request& request, fs 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, request.vehicle_name); + 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()); diff --git a/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv b/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv index 622ca59f..1086d919 100644 --- a/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv +++ b/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv @@ -1,9 +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 +float32 x +float32 y +float32 z +float32 yaw # in degree string vehicle_name --- #Response : success=true - (if async=false && if setpoint reached before timeout = 30sec) || (if async=true) From 85e24409fbc2bc671dcf34c64fca749b7c7bc5e8 Mon Sep 17 00:00:00 2001 From: sijmenhuizenga Date: Sun, 17 Jan 2021 22:39:15 +0100 Subject: [PATCH 2/2] Removed vehicle_name from SetLocalPosition.srv as it FSDS only supports 1 vehicle anyway --- ros/src/fsds_ros_bridge/CMakeLists.txt | 7 +++++++ ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp | 2 +- ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv | 1 - 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ros/src/fsds_ros_bridge/CMakeLists.txt b/ros/src/fsds_ros_bridge/CMakeLists.txt index 4e510103..9261e91a 100644 --- a/ros/src/fsds_ros_bridge/CMakeLists.txt +++ b/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 diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp index bf73b540..dcaa3c64 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -287,7 +287,7 @@ bool AirsimROSWrapper::set_local_position_srv_cb(fsds_ros_bridge::SetLocalPositi 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, request.vehicle_name); + airsim_client_.simSetVehiclePose(msr::airlib::Pose {vec3r, quaternion}, true, "FSCar"); return true; } diff --git a/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv b/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv index 1086d919..e764164d 100644 --- a/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv +++ b/ros/src/fsds_ros_bridge/srv/SetLocalPosition.srv @@ -4,7 +4,6 @@ float32 x float32 y float32 z float32 yaw # in degree -string vehicle_name --- #Response : success=true - (if async=false && if setpoint reached before timeout = 30sec) || (if async=true) bool success