Feature/pid controller

CMakeLists.txt

@@ -45,6 +45,7 @@ cs_add_library(${PROJECT_NAME}
         src/simulator_processing/infrared_id_compensation.cpp
         src/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.cpp
         src/simulator_processing/odometry_drift_simulator/normal_distribution.cpp
+        src/simulator_processing/pid_controller.cpp
         )
 
 ###############

app/tools/parse_config_to_airsim.py

@@ -274,9 +274,9 @@ def parse_transformation(self, list_in, dict_out):
             R = [x[:3] for x in list_in[:3]]
             # Support python 2 and 3 scipy versions.
             if sys.version_info >= (3, 0):
-                R = Rotation.from_dcm(R)
-            else:
                 R = Rotation.from_matrix(R)
+            else:
+                R = Rotation.from_dcm(R)
             euler = R.as_euler(
                 'xyz', degrees=True
             )  # Airsim is quite inconsistent here with rotation parametrization

include/unreal_airsim/frame_converter.h

@@ -42,9 +42,11 @@ class FrameConverter {
 
   void rosToAirsim(Eigen::Vector3d* point) const;
   void rosToAirsim(geometry_msgs::Point* point) const;
+  void rosToAirsim(geometry_msgs::Vector3* vector) const;
   void rosToAirsim(Eigen::Quaterniond* orientation) const;
   void rosToAirsim(geometry_msgs::Quaternion* orientation) const;
   void rosToAirsim(geometry_msgs::Pose* pose) const;
+  void rosToAirsim(geometry_msgs::Transform* pose) const;
 
   // transformations
   void transformPointAirsimToRos(double* x, double* y, double* z) const;

include/unreal_airsim/math_utils.h

@@ -0,0 +1,57 @@
+#ifndef UNREAL_AIRSIM_MATH_UTILS_H_
+#define UNREAL_AIRSIM_MATH_UTILS_H_
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace math_utils {
+
+template <typename T>
+inline T radToDeg(const T radians) {
+  return (radians / M_PI) * 180.0;
+}
+
+template <typename T>
+inline T degToRad(const T degrees) {
+  return (degrees / 180.0) * M_PI;
+}
+
+template <typename T>
+inline T wrapToPi(T radians) {
+  int m = (int)(radians / (2 * M_PI));
+  radians = radians - m * 2 * M_PI;
+  if (radians > M_PI)
+    radians -= 2.0 * M_PI;
+  else if (radians < -M_PI)
+    radians += 2.0 * M_PI;
+  return radians;
+}
+
+template <typename T>
+inline void wrapToPiInplace(T& a) {
+  a = wrapToPi(a);
+}
+
+template <class T>
+inline T angularDistance(T from, T to) {
+  wrapToPiInplace(from);
+  wrapToPiInplace(to);
+  T d = to - from;
+  if (d > M_PI)
+    d -= 2. * M_PI;
+  else if (d < -M_PI)
+    d += 2. * M_PI;
+  return d;
+}
+
+inline double getYawFromQuaternionMsg(const geometry_msgs::Quaternion& quat_msg) {
+  tf2::Quaternion quat_tf;
+  double roll, pitch, yaw;
+  tf2::fromMsg(quat_msg, quat_tf);
+  tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw);
+  return yaw;
+}
+}  // namespace math_utils
+
+#endif  // UNREAL_AIRSIM_MATH_UTILS_H_

include/unreal_airsim/online_simulator/simulator.h

@@ -2,26 +2,39 @@
 #define UNREAL_AIRSIM_ONLINE_SIMULATOR_SIMULATOR_H_
 
 #include <memory>
+#include <queue>
 #include <string>
 #include <thread>
 #include <vector>
 
+#include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 #include <std_msgs/Time.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <trajectory_msgs/MultiDOFJointTrajectory.h>
 
 // AirSim
 #include <common/CommonStructs.hpp>
 #include <vehicles/multirotor/api/MultirotorRpcLibClient.hpp>
 
 #include "unreal_airsim/frame_converter.h"
 #include "unreal_airsim/online_simulator/sensor_timer.h"
+#include "unreal_airsim/simulator_processing/pid_controller.h"
 #include "unreal_airsim/simulator_processing/processor_base.h"
 
 #include "unreal_airsim/simulator_processing/odometry_drift_simulator/odometry_drift_simulator.h"
 
 namespace unreal_airsim {
+
+typedef struct Waypoint {
+  geometry_msgs::Transform_<std::allocator<void>> pose;
+  bool isGoal{};
+} Waypoint;
+
 /***
  * This class implements a simulation interface with airsim.
  * Current application case is for a single Multirotor Vehicle.
@@ -49,11 +62,13 @@ class AirsimSimulator {
         msr::airlib::DrivetrainType::MaxDegreeOfFreedom;  // this is currently
                                                           // fixed
 
+    float reset_timer = 5.f; // s, time to let AirSim settle before resetting.
     // sensors
     bool publish_sensor_transforms = true;  // publish transforms when receiving
     // sensor measurements to guarantee correct tfs.
     // TODO(schmluk): This is mostly a time syncing problem, maybe easiest to
     //  publish the body pose based on these readings.
+
     struct Sensor {
       inline static const std::string TYPE_CAMERA = "Camera";
       inline static const std::string TYPE_LIDAR = "Lidar";
@@ -70,13 +85,15 @@ class AirsimSimulator {
       Eigen::Vector3d translation;  // T_B_S, default is unit transform
       Eigen::Quaterniond rotation;
     };
+
     struct Camera : Sensor {
       std::string image_type_str = "Scene";
       bool pixels_as_float = false;
       msr::airlib::ImageCaptureBase::ImageType image_type =
           msr::airlib::ImageCaptureBase::ImageType::Scene;
       msr::airlib::CameraInfo camera_info;  // The info is read from UE4
     };
+
     std::vector<std::unique_ptr<Sensor>> sensors;
   };
 
@@ -96,6 +113,12 @@ class AirsimSimulator {
    */
   void commandPoseCallback(const geometry_msgs::Pose& msg);
 
+  // added from trajectory caller node
+  void commandTrajectorycallback(
+      const trajectory_msgs::MultiDOFJointTrajectory trajectory);
+
+  void trackWayPoints();
+
   // Acessors
   const Config& getConfig() const { return config_; }
   const FrameConverter& getFrameConverter() const { return frame_converter_; }
@@ -116,15 +139,29 @@ class AirsimSimulator {
   ros::Publisher sim_is_ready_pub_;
   ros::Publisher time_pub_;
   ros::Subscriber command_pose_sub_;
+  ros::Subscriber command_trajectory_sub_;
   tf2_ros::TransformBroadcaster tf_broadcaster_;
   tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
 
   // Odometry simulator
   OdometryDriftSimulator odometry_drift_simulator_;
+  std::queue<Waypoint*> points;
+  Waypoint* current_goal;
+  // Whether robot is idle or currently tracking a goal
+  bool followingGoal;
+  bool only_move_in_yaw_direction;
+  // Current yaw of the goal position
+  double goal_yaw;
 
   // Read sim time from AirSim
   std::thread timer_thread_;
 
+  bool reached_goal_;
+  bool has_goal_;
+  bool got_goal_once_;
+
+  PIDPositionController pid_controller_;
+
   // components
   std::vector<std::unique_ptr<SensorTimer>>
       sensor_timers_;  // These manage the actual sensor reading/publishing

include/unreal_airsim/simulator_processing/depth_to_pointcloud.h

@@ -40,7 +40,7 @@ class DepthToPointcloud : public ProcessorBase {
   ros::Subscriber segmentation_sub_;
 
   // queues
-  std::mutex queue_guard;
+  std::mutex queue_mutex_;
   std::deque<sensor_msgs::ImagePtr> depth_queue_;
   std::deque<sensor_msgs::ImagePtr> color_queue_;
   std::deque<sensor_msgs::ImagePtr> segmentation_queue_;

include/unreal_airsim/simulator_processing/pid_controller.h

@@ -0,0 +1,129 @@
+/**
+ * NOTE: This class was largely taken and adapted from Microsoft AirSim's Simple
+ * PD controller.
+ * (https://github.com/microsoft/AirSim/blob/master/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp)
+ *
+ */
+
+#ifndef UNREAL_AIRSIM_PID_CONTROLLER_H_
+#define UNREAL_AIRSIM_PID_CONTROLLER_H_
+
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <common/CommonStructs.hpp>
+#include <vehicles/multirotor/api/MultirotorRpcLibClient.hpp>
+
+#include "unreal_airsim/math_utils.h"
+
+namespace unreal_airsim {
+
+class PIDParams {
+ public:
+  double kp_x;
+  double kp_y;
+  double kp_z;
+  double kp_yaw;
+  double kd_x;
+  double kd_y;
+  double kd_z;
+  double kd_yaw;
+
+  double reached_thresh_xyz;
+  double reached_yaw_degrees;
+
+  PIDParams()
+      : kp_x(0.5),
+        kp_y(0.5),
+        kp_z(0.10),
+        kp_yaw(0.4),
+        kd_x(0.3),
+        kd_y(0.3),
+        kd_z(0.2),
+        kd_yaw(0.1),
+        reached_thresh_xyz(0.1),
+        reached_yaw_degrees(0.1) {}
+
+  bool load_from_rosparams(const ros::NodeHandle& nh);
+};
+
+// todo should be a common representation
+struct XYZYaw {
+  double x;
+  double y;
+  double z;
+  double yaw;
+};
+
+// todo should be a common representation
+class DynamicConstraints {
+ public:
+  double max_vel_horz_abs;  // meters/sec
+  double max_vel_vert_abs;
+  double max_yaw_rate_degree;
+
+  DynamicConstraints()
+      : max_vel_horz_abs(1.0),
+        max_vel_vert_abs(0.5),
+        max_yaw_rate_degree(10.0) {}
+
+  bool load_from_rosparams(const ros::NodeHandle& nh);
+};
+
+class PIDPositionController {
+ public:
+  PIDPositionController(const ros::NodeHandle& nh,
+                        const ros::NodeHandle& nh_private);
+
+  // ROS service callbacks
+  // bool local_position_goal_srv_cb(airsim_ros_pkgs::SetLocalPosition::Request&
+  // request, airsim_ros_pkgs::SetLocalPosition::Response& response);
+  // ROS subscriber callbacks
+  // void airsim_odom_cb(const nav_msgs::Odometry& odom_msg);
+  // void set_airsim_odom(const geometry_msgs::Pose& pose);
+  void set_yaw_position(const double x, const double y, const double z,
+                        const double yaw);
+  bool set_target(const double x, const double y, const double z,
+                  const double yaw);
+  void update_control_cmd_timer_cb(const ros::TimerEvent& event);
+  void tick();
+  void reset_errors();
+
+  void initialize_ros();
+  void compute_control_cmd();
+  void enforce_dynamic_constraints();
+  void publish_control_cmd();
+  void check_reached_goal();
+  bool is_goal_reached() const;
+  bool get_velocity(geometry_msgs::Twist&) const;
+  msr::airlib::YawMode getYawMode() const;
+
+ private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  DynamicConstraints constraints_;
+  PIDParams params_;
+  XYZYaw target_position_;
+  XYZYaw curr_position_;
+  XYZYaw prev_error_;
+  XYZYaw curr_error_;
+
+  nav_msgs::Odometry curr_odom_;
+  geometry_msgs::Twist vel_cmd_;
+  bool reached_goal_;
+  bool has_goal_;
+  bool has_odom_;
+  bool got_goal_once_;
+  // todo check for odom msg being older than n sec
+
+  ros::Publisher airsim_vel_cmd_world_frame_pub_;
+  ros::Subscriber airsim_odom_sub_;
+  ros::ServiceServer local_position_goal_srvr_;
+  ros::Timer update_control_cmd_timer_;
+};
+
+}  // namespace unreal_airsim
+#endif  // UNREAL_AIRSIM_PID_CONTROLLER_H_

launch/airsim_simulator.launch

@@ -14,26 +14,18 @@
 
 
 
-
   <!-- *** Run the Simulation *** -->
 
-
-
-
   <!-- static camera transform -->
   <node pkg="tf" type="static_transform_publisher" name="tf_camera_to_link" args="0 0 0 -0.5 0.5 -0.5 0.5 /camera_link /airsim_camera 100"/>  
   <node pkg="tf" type="static_transform_publisher" name="tf_odom_to_world" args="0 0 0 0 0 0 1 /world /odom 100"/>  
-<!--  <node pkg="tf" type="static_transform_publisher" name="tf_odom_to_base" args="0 0 0 0 0 0 1 /camera_link /base_link 100"/>  -->
-<!--  <node pkg="tf" type="static_transform_publisher" name="tf_cam_to_base" args="0 0 0 0 0 0 1 /airsim_camera /base_link 100"/>  -->
   <node pkg="tf" type="static_transform_publisher" name="tf_mission_to_world" args="0 0 0 0 0 0 1 /mission /world 100"/>  
 
-
   <!-- airsim client -->
   <node name="airsim_simulator" pkg="unreal_airsim" type="airsim_simulator_node" required="true" output="screen" args="-alsologtostderr">
      <rosparam file="$(find unreal_airsim)/cfg/airsim_simulation.yaml"/>
   </node>
 
-
 <!--     Voxblox -->
 <!--  <node name="voxblox_node" pkg="voxblox_ros" type="esdf_server" output="screen" args="-alsologtostderr">-->
 <!--    <remap from="pointcloud" to="/unreal/online_perception/pointcloud"/>-->
@@ -55,7 +47,6 @@
 <!--    <rosparam file="$(find unreal_airsim)/cfg/voxgraph_mapper.yaml"/>-->
 <!--    <remap from="velodyne_points" to="/unreal/online_perception/pointcloud"/>-->
 <!--  </node>-->
-
 
   <!-- RVIZ Visualization -->
   <node type="rviz" name="rviz_voxblox" pkg="rviz" args="-d $(find unreal_airsim)/cfg/test_airsim.rviz"/>

src/frame_converter.cpp

@@ -118,6 +118,10 @@ void FrameConverter::rosToAirsim(geometry_msgs::Point* point) const {
   transformPointRosToAirsim(&(point->x), &(point->y), &(point->z));
 }
 
+void FrameConverter::rosToAirsim(geometry_msgs::Vector3* vector) const {
+  transformPointRosToAirsim(&(vector->x), &(vector->y), &(vector->z));
+}
+
 void FrameConverter::rosToAirsim(geometry_msgs::Quaternion* orientation) const {
   transformOrientationRosToAirsim(&(orientation->w), &(orientation->x),
                                   &(orientation->y), &(orientation->z));
@@ -133,4 +137,9 @@ void FrameConverter::rosToAirsim(geometry_msgs::Pose* pose) const {
   rosToAirsim(&(pose->orientation));
 }
 
+void FrameConverter::rosToAirsim(geometry_msgs::Transform* pose) const {
+  rosToAirsim(&(pose->translation));
+  rosToAirsim(&(pose->rotation));
+}
+
 }  // namespace unreal_airsim