[ros] Upgrade to setCameraPose

microsoft · Jul 22, 2020 · a6033cd · a6033cd
1 parent a357832
commit a6033cd
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
@@ -281,6 +281,7 @@ class AirsimROSWrapper
     nav_msgs::Odometry get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const;
     nav_msgs::Odometry get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const;
     airsim_ros_pkgs::CarState get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const;
+    msr::airlib::Pose get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const;
     airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const;
     sensor_msgs::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const;
     sensor_msgs::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const;

diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
@@ -482,6 +482,11 @@ void AirsimROSWrapper::car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr&
     car->has_car_cmd = true;
 }
 
+msr::airlib::Pose AirsimROSWrapper::get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const
+{
+    return msr::airlib::Pose(msr::airlib::Vector3r(x, y, z), airlib_quat);
+}
+
 // void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg, const std::string& vehicle_name)
 void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name)
 {
@@ -626,7 +631,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng
 // todo support multiple gimbal commands
 // 1. find quaternion of default gimbal pose
 // 2. forward multiply with quaternion equivalent to desired euler commands (in degrees)
-// 3. call airsim client's setcameraorientation which sets camera orientation wrt world (or takeoff?) ned frame. todo 
+// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo 
 void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg)
 {
     try
@@ -1181,11 +1186,11 @@ void AirsimROSWrapper::update_commands()
         }
     }
 
-    // todo add and expose a gimbal angular velocity to airlib
+    // Only camera rotation, no translation movement of camera 
     if (has_gimbal_cmd_)
     {
         std::lock_guard<std::mutex> guard(drone_control_mutex_);
-        airsim_client_->simSetCameraOrientation(gimbal_cmd_.camera_name, gimbal_cmd_.target_quat, gimbal_cmd_.vehicle_name);
+        airsim_client_.simSetCameraPose(gimbal_cmd_.camera_name, get_airlib_pose(0, 0, 0, gimbal_cmd_.target_quat), gimbal_cmd_.vehicle_name);
     }
 
     has_gimbal_cmd_ = false;