From b3685240785333c4183f69b8fd55d7bf70518538 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 10:19:35 +0100 Subject: [PATCH 01/16] first part of reviewed changes --- CMakeLists.txt | 1 + include/unreal_airsim/frame_converter.h | 2 ++ .../simulator_processing/depth_to_pointcloud.h | 1 + src/frame_converter.cpp | 9 +++++++++ 4 files changed, 13 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c05459..744d09a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ catkin_package() cs_add_library(${PROJECT_NAME} # Modules src/frame_converter.cpp + src/pid_controller.cpp src/online_simulator/simulator.cpp src/online_simulator/sensor_timer.cpp src/simulator_processing/processor_factory.cpp diff --git a/include/unreal_airsim/frame_converter.h b/include/unreal_airsim/frame_converter.h index 8e3342e..48ba5f5 100644 --- a/include/unreal_airsim/frame_converter.h +++ b/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; diff --git a/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h b/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h index 8202336..f9dd078 100644 --- a/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h +++ b/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h @@ -44,6 +44,7 @@ class DepthToPointcloud : public ProcessorBase { std::deque depth_queue_; std::deque color_queue_; std::deque segmentation_queue_; + std::mutex queue_guard; // variables bool use_color_; diff --git a/src/frame_converter.cpp b/src/frame_converter.cpp index 2a23988..4d146a2 100644 --- a/src/frame_converter.cpp +++ b/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 From 5774fa6a8c3daf4bb83e803632254360dbbb1b79 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 10:21:23 +0100 Subject: [PATCH 02/16] update launch file --- launch/airsim_simulator.launch | 9 --------- 1 file changed, 9 deletions(-) diff --git a/launch/airsim_simulator.launch b/launch/airsim_simulator.launch index 4edb94f..e6d7d49 100644 --- a/launch/airsim_simulator.launch +++ b/launch/airsim_simulator.launch @@ -14,26 +14,18 @@ - - - - - - - - @@ -55,7 +47,6 @@ - From bdb206dc5fe346a3e915c6edb28994eab93d79d6 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 10:24:18 +0100 Subject: [PATCH 03/16] refactor math utils --- include/unreal_airsim/math_utils.h | 57 ++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 include/unreal_airsim/math_utils.h diff --git a/include/unreal_airsim/math_utils.h b/include/unreal_airsim/math_utils.h new file mode 100644 index 0000000..9beef36 --- /dev/null +++ b/include/unreal_airsim/math_utils.h @@ -0,0 +1,57 @@ +#ifndef UNREAL_AIRSIM_MATH_UTILS_H_ +#define UNREAL_AIRSIM_MATH_UTILS_H_ + +#include +#include +#include + +namespace math_utils { + +template +inline T radToDeg(const T radians) { + return (radians / M_PI) * 180.0; +} + +template +inline T degToRad(const T degrees) { + return (degrees / 180.0) * M_PI; +} + +template +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 +inline void wrapToPiInplace(T& a) { + a = wrapToPi(a); +} + +template +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_ \ No newline at end of file From 07bc2793ac925ddd22511893e833bc46cacb29ba Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 10:56:00 +0100 Subject: [PATCH 04/16] update depth cloud processing to be thread safe --- .../depth_to_pointcloud.cpp | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/simulator_processing/depth_to_pointcloud.cpp b/src/simulator_processing/depth_to_pointcloud.cpp index 422eea6..9a7f294 100644 --- a/src/simulator_processing/depth_to_pointcloud.cpp +++ b/src/simulator_processing/depth_to_pointcloud.cpp @@ -147,9 +147,10 @@ void DepthToPointcloud::findMatchingMessagesToPublish( // If a modality is not used we count it as found. bool found_color = true; bool found_segmentation = true; - - std::lock_guard guard(queue_guard); + // Make this blocking since this process alters the queue elements in multiple + // threads. + std::lock_guard guard(queue_guard); depth_it = std::find_if(depth_queue_.begin(), depth_queue_.end(), [reference_msg](const sensor_msgs::ImagePtr& s) { @@ -187,6 +188,13 @@ void DepthToPointcloud::findMatchingMessagesToPublish( if (found_color && found_segmentation) { publishPointcloud(*depth_it, use_color_ ? *color_it : nullptr, use_segmentation_ ? *segmentation_it : nullptr); + if (std::find(depth_queue_.begin(), depth_queue_.end(), *depth_it) == + depth_queue_.end()) { + // Double check the depth image is still there. + LOG(WARNING) + << "Tried to remove a depth image that is no longer in the queue."; + return; + } depth_queue_.erase(depth_it); if (use_color_) { color_queue_.erase(color_it); @@ -214,13 +222,19 @@ void DepthToPointcloud::publishPointcloud( return; } cv_bridge::CvImageConstPtr depth_img, color_img, segmentation_img; - depth_img = cv_bridge::toCvShare(depth_ptr, depth_ptr->encoding); - if (use_color_) { - color_img = cv_bridge::toCvShare(color_ptr, color_ptr->encoding); - } - if (use_segmentation_) { - segmentation_img = - cv_bridge::toCvShare(segmentation_ptr, segmentation_ptr->encoding); + try { + depth_img = cv_bridge::toCvShare(depth_ptr, depth_ptr->encoding); + if (use_color_) { + color_img = cv_bridge::toCvShare(color_ptr, color_ptr->encoding); + } + if (use_segmentation_) { + segmentation_img = + cv_bridge::toCvShare(segmentation_ptr, segmentation_ptr->encoding); + } + } catch (const cv_bridge::Exception& e) { + LOG(WARNING) << "Failed to read input images (" << e.what() + << "), skipping frame."; + return; } // figure out number of points From f42f23659d547f235a68de995f67986ea38fe26a Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 12:29:27 +0100 Subject: [PATCH 05/16] partial merge --- CMakeLists.txt | 2 +- .../infrared_id_compensation.cpp | 16 ++++++++++------ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 744d09a..4fefa8c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,7 +38,6 @@ catkin_package() cs_add_library(${PROJECT_NAME} # Modules src/frame_converter.cpp - src/pid_controller.cpp src/online_simulator/simulator.cpp src/online_simulator/sensor_timer.cpp src/simulator_processing/processor_factory.cpp @@ -46,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 ) ############### diff --git a/src/simulator_processing/infrared_id_compensation.cpp b/src/simulator_processing/infrared_id_compensation.cpp index f65a40b..ec4f659 100644 --- a/src/simulator_processing/infrared_id_compensation.cpp +++ b/src/simulator_processing/infrared_id_compensation.cpp @@ -54,14 +54,18 @@ bool InfraredIdCompensation::setupFromRos(const ros::NodeHandle& nh, } void InfraredIdCompensation::imageCallback(const sensor_msgs::ImagePtr& msg) { - cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, msg->encoding); - for (int v = 0; v < img->image.rows; v++) { - for (int u = 0; u < img->image.cols; u++) { - img->image.at(v, u) = - infrared_compensation_[img->image.at(v, u)]; + try { + cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, msg->encoding); + for (int v = 0; v < img->image.rows; v++) { + for (int u = 0; u < img->image.cols; u++) { + img->image.at(v, u) = + infrared_compensation_[img->image.at(v, u)]; + } } + pub_.publish(img->toImageMsg()); + } catch (const cv_bridge::Exception& e) { + LOG(WARNING) << "Could not convert image to CvImage (" << e.what() << "), skipping frame."; } - pub_.publish(img->toImageMsg()); } } // namespace unreal_airsim::simulator_processor From a73e26c986cc92489040ae551e8ca589acde0036 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 12:41:31 +0100 Subject: [PATCH 06/16] preliminary PID controller version --- .../online_simulator/simulator.h | 33 +++ .../depth_to_pointcloud.h | 3 +- .../simulator_processing/pid_controller.h | 129 ++++++++++ src/online_simulator/simulator.cpp | 161 +++++++++++- .../depth_to_pointcloud.cpp | 2 +- src/simulator_processing/pid_controller.cpp | 229 ++++++++++++++++++ 6 files changed, 549 insertions(+), 8 deletions(-) create mode 100644 include/unreal_airsim/simulator_processing/pid_controller.h create mode 100644 src/simulator_processing/pid_controller.cpp diff --git a/include/unreal_airsim/online_simulator/simulator.h b/include/unreal_airsim/online_simulator/simulator.h index 7ad80d9..7802e8c 100644 --- a/include/unreal_airsim/online_simulator/simulator.h +++ b/include/unreal_airsim/online_simulator/simulator.h @@ -2,14 +2,20 @@ #define UNREAL_AIRSIM_ONLINE_SIMULATOR_SIMULATOR_H_ #include +#include #include #include #include +#include #include #include +#include +#include +#include #include #include +#include // AirSim #include @@ -17,11 +23,18 @@ #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_> pose; + bool isGoal{}; +} Waypoint; + /*** * This class implements a simulation interface with airsim. * Current application case is for a single Multirotor Vehicle. @@ -96,6 +109,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 +135,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 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> sensor_timers_; // These manage the actual sensor reading/publishing diff --git a/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h b/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h index f9dd078..de9e3b1 100644 --- a/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h +++ b/include/unreal_airsim/simulator_processing/depth_to_pointcloud.h @@ -40,11 +40,10 @@ class DepthToPointcloud : public ProcessorBase { ros::Subscriber segmentation_sub_; // queues - std::mutex queue_guard; + std::mutex queue_mutex_; std::deque depth_queue_; std::deque color_queue_; std::deque segmentation_queue_; - std::mutex queue_guard; // variables bool use_color_; diff --git a/include/unreal_airsim/simulator_processing/pid_controller.h b/include/unreal_airsim/simulator_processing/pid_controller.h new file mode 100644 index 0000000..a1231eb --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#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_ diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 4aa4070..622abd2 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -20,6 +20,7 @@ STRICT_MODE_ON #include #include #include +#include #include @@ -38,6 +39,11 @@ AirsimSimulator::AirsimSimulator(const ros::NodeHandle& nh, is_connected_(false), is_running_(false), is_shutdown_(false), + followingGoal(false), + only_move_in_yaw_direction(true), + goal_yaw(0), + has_goal_(false), + pid_controller_(nh, nh_private), odometry_drift_simulator_( OdometryDriftSimulator::Config::fromRosParams(nh_private)) { // configure @@ -56,11 +62,13 @@ AirsimSimulator::AirsimSimulator(const ros::NodeHandle& nh, // setup everything initializeSimulationFrame(); setupROS(); + LOG(INFO) << "setup ROS"; startSimTimer(); - + LOG(INFO) << "setting up timer."; // Startup the vehicle simulation via callback startup_timer_ = nh_private_.createTimer( ros::Duration(0.1), &AirsimSimulator::startupCallback, this); + LOG(INFO) << "Setup timer."; } bool AirsimSimulator::readParamsFromRos() { @@ -281,7 +289,11 @@ bool AirsimSimulator::setupROS() { nh_.subscribe(config_.vehicle_name + "/command/pose", 10, &AirsimSimulator::commandPoseCallback, this); - // sensors + command_trajectory_sub_ = + nh_.subscribe(config_.vehicle_name + "/command/trajectory", 10, + &AirsimSimulator::commandTrajectorycallback, this); + + // // sensors for (size_t i = 0; i < config_.sensors.size(); ++i) { // Find or allocate the sensor timer SensorTimer* timer = nullptr; @@ -405,6 +417,40 @@ bool AirsimSimulator::initializeSimulationFrame() { return true; } +// Callback for trajectories published by active plner +void AirsimSimulator::commandTrajectorycallback( + const trajectory_msgs::MultiDOFJointTrajectory trajectory) { + if (!is_running_) { + return; + } + + for (const auto& point : trajectory.points) { + for (auto pose : point.transforms) { + + frame_converter_.rosToAirsim(&pose); + + auto* pt = new Waypoint; + pt->pose = pose; + pt->isGoal = false; + points.push(pt); + } + } + // Add goal point for yaw tracking as last point + auto* lastPose = points.back(); + auto* pt = new Waypoint; + pt->pose.translation.x = lastPose->pose.translation.x; + pt->pose.translation.y = lastPose->pose.translation.y; + pt->pose.translation.z = lastPose->pose.translation.z; + + pt->pose.rotation.y = lastPose->pose.rotation.y; + pt->pose.rotation.x = lastPose->pose.rotation.x; + pt->pose.rotation.z = lastPose->pose.rotation.z; + pt->pose.rotation.w = lastPose->pose.rotation.w; + pt->isGoal = true; + points.push(pt); + +} + void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { if (!is_running_) { return; @@ -422,28 +468,43 @@ void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { // Use position + yaw as setpoint auto command_pos = T_gt_command.getPosition(); + auto command_ori = T_gt_command.getEigenQuaternion(); + std::cout << "Pose Orientation In ROS Frame: " + << command_ori.x() << " " + << command_ori.y() << " " + << command_ori.z() << " " + << command_ori.w() << std::endl; frame_converter_.rosToAirsim(&command_ori); + std::cout << "Pose Orientation In Airsim Frame: " + << command_ori.x() << " " + << command_ori.y() << " " + << command_ori.z() << " " + << command_ori.w() << std::endl; double yaw = tf2::getYaw( tf2::Quaternion(command_ori.x(), command_ori.y(), command_ori.z(), command_ori.w())); // Eigen's eulerAngles apparently // messes up some wrap arounds or direcions and gets the wrong yaws in some // cases + //config_.velocity =0.2f; yaw = yaw / M_PI * 180.0; constexpr double kMinMovingDistance = 0.1; // m airsim_move_client_.cancelLastTask(); if ((command_pos - t_gt_current_position).norm() >= kMinMovingDistance) { frame_converter_.rosToAirsim(&command_pos); auto yaw_mode = msr::airlib::YawMode(false, yaw); + float look_ahead=-1, adaptive_lookahead=0; airsim_move_client_.moveToPositionAsync( command_pos.x(), command_pos.y(), command_pos.z(), config_.velocity, - 3600, config_.drive_train_type, yaw_mode, -1, 1, config_.vehicle_name); + 3600, config_.drive_train_type, yaw_mode, look_ahead,adaptive_lookahead, config_.vehicle_name); + std::cout <<"Moving to target position with "<pose.translation; + + double yaw; + + if (only_move_in_yaw_direction && !current_goal->isGoal) { + // We are currently tracking waypoints and not final goalpoints + // If only move in yaw direction, set yaw of waypoints (except goal + // point) to moving direction + auto delta_x = target_position.x - current_position.x; + auto delta_y = target_position.y - current_position.y; + if (abs(delta_x) + abs(delta_y) <= 0.1) { + // If we overshoot over the goalpoint, the robot will rotate 180 deg + // and drive back for a really small amount. Check if overshoot + // occured (delta really small) and don't turn around in this case + yaw = goal_yaw; + } else { + yaw = atan2(delta_y, delta_x); + } + + } else { + // Rotate to goal yaw + tf::Quaternion q( + current_goal->pose.rotation.x, current_goal->pose.rotation.y, + current_goal->pose.rotation.z, current_goal->pose.rotation.w); + tf::Matrix3x3 m(q); + double roll, pitch; + m.getRPY(roll, pitch, yaw); + } + goal_yaw = yaw; + + // goal_received=true + + if (pid_controller_.set_target(target_position.x, target_position.y, + target_position.z, goal_yaw)) { + followingGoal = true; + } + + } else { // already following a goal + bool goal_reached = pid_controller_.is_goal_reached(); + + if (goal_reached) { + + // move to next way point + auto* off = points.front(); + points.pop(); + delete off; + followingGoal = false; + } + } + + pid_controller_.tick(); + + geometry_msgs::Twist vel_cmd; + if (pid_controller_.get_velocity(vel_cmd)) { + double vel_cmd_duration = 1.0 / config_.state_refresh_rate; + // move the drone + airsim_move_client_.moveByVelocityAsync( + vel_cmd.linear.x, vel_cmd.linear.y, vel_cmd.linear.z, vel_cmd_duration, + msr::airlib::DrivetrainType::MaxDegreeOfFreedom, + pid_controller_.getYawMode(), config_.vehicle_name); + } + } +} + void AirsimSimulator::simStateCallback(const ros::TimerEvent&) { if (is_shutdown_) { return; @@ -538,7 +685,7 @@ void AirsimSimulator::simStateCallback(const ros::TimerEvent&) { airsim_state_client_.getMultirotorState(config_.vehicle_name); ros::Time stamp = getTimeStamp(state.timestamp); - // convert airsim pose to ROS + //convert airsim pose to ROS geometry_msgs::TransformStamped transformStamped; transformStamped.header.stamp = stamp; transformStamped.header.frame_id = config_.simulator_frame_name; @@ -550,7 +697,7 @@ void AirsimSimulator::simStateCallback(const ros::TimerEvent&) { transformStamped.transform.rotation); frame_converter_.airsimToRos(&transformStamped.transform); - // simulate odometry drift + //simulate odometry drift odometry_drift_simulator_.tick(transformStamped); // publish TFs, odom msgs and pose msgs @@ -597,8 +744,12 @@ void AirsimSimulator::simStateCallback(const ros::TimerEvent&) { msg.data = true; collision_pub_.publish(msg); } + // track trajectory + trackWayPoints(); } + + bool AirsimSimulator::readTransformFromRos(const std::string& topic, Eigen::Vector3d* translation, Eigen::Quaterniond* rotation) { diff --git a/src/simulator_processing/depth_to_pointcloud.cpp b/src/simulator_processing/depth_to_pointcloud.cpp index 9a7f294..0dd4311 100644 --- a/src/simulator_processing/depth_to_pointcloud.cpp +++ b/src/simulator_processing/depth_to_pointcloud.cpp @@ -150,7 +150,7 @@ void DepthToPointcloud::findMatchingMessagesToPublish( // Make this blocking since this process alters the queue elements in multiple // threads. - std::lock_guard guard(queue_guard); + std::lock_guard guard(queue_mutex_); depth_it = std::find_if(depth_queue_.begin(), depth_queue_.end(), [reference_msg](const sensor_msgs::ImagePtr& s) { diff --git a/src/simulator_processing/pid_controller.cpp b/src/simulator_processing/pid_controller.cpp new file mode 100644 index 0000000..af0c706 --- /dev/null +++ b/src/simulator_processing/pid_controller.cpp @@ -0,0 +1,229 @@ +#include "unreal_airsim/simulator_processing/pid_controller.h" + +namespace unreal_airsim { + +bool PIDParams::load_from_rosparams(const ros::NodeHandle& nh) { + bool found = true; + + found = nh.getParam("kp_x", kp_x) && found; + found = nh.getParam("kp_y", kp_y) && found; + found = nh.getParam("kp_z", kp_z) && found; + found = nh.getParam("kp_yaw", kp_yaw) && found; + + found = nh.getParam("kd_x", kd_x) && found; + found = nh.getParam("kd_y", kd_y) && found; + found = nh.getParam("kd_z", kd_z) && found; + found = nh.getParam("kd_yaw", kd_yaw) && found; + + found = nh.getParam("reached_thresh_xyz", reached_yaw_degrees) && found; + found = nh.getParam("reached_yaw_degrees", reached_yaw_degrees) && found; + + return found; +} + +bool DynamicConstraints::load_from_rosparams(const ros::NodeHandle& nh) { + bool found = true; + + found = nh.getParam("max_vel_horz_abs", max_vel_horz_abs) && found; + found = nh.getParam("max_vel_vert_abs", max_vel_vert_abs) && found; + found = nh.getParam("max_yaw_rate_degree", max_yaw_rate_degree) && found; + + return found; +} + +PIDPositionController::PIDPositionController(const ros::NodeHandle& nh, + const ros::NodeHandle& nh_private) + : nh_(nh), + nh_private_(nh_private), + has_odom_(false), + has_goal_(false), + reached_goal_(false), + got_goal_once_(false) { + params_.load_from_rosparams(nh_private_); + constraints_.load_from_rosparams(nh_); + initialize_ros(); + reset_errors(); +} + +void PIDPositionController::reset_errors() { + prev_error_.x = 0.0; + prev_error_.y = 0.0; + prev_error_.z = 0.0; + prev_error_.yaw = 0.0; +} + +void PIDPositionController::initialize_ros() {} + +void PIDPositionController::set_yaw_position(const double x, const double y, + const double z, const double yaw) { + has_odom_ = true; + curr_position_.x = x; + curr_position_.y = y; + curr_position_.z = z; + curr_position_.yaw = yaw; +} + +bool PIDPositionController::set_target(const double x, const double y, + const double z, const double yaw) { + if (!got_goal_once_) got_goal_once_ = true; + + if (has_goal_ && !reached_goal_) { + // todo maintain array of position goals + ROS_ERROR_STREAM( + "[PIDPositionController] denying position goal request. I am still " + "following the previous goal"); + return false; + } + + if (!has_goal_) { + target_position_.x = x; + target_position_.y = y; + target_position_.z = z; + target_position_.yaw = yaw; + + // ROS_INFO_STREAM("[PIDPositionController] got goal: x=" << + // target_position_.x << " y=" << target_position_.y << " z=" << + // target_position_.z << " yaw=" << target_position_.yaw); + + // todo error checks + // todo fill response + has_goal_ = true; + reached_goal_ = false; + reset_errors(); // todo + return true; + } + + // Already have goal, and have reached it + // ROS_INFO_STREAM("[PIDPositionController] Already have goal and have reached + // it"); + return false; +} + +void PIDPositionController::check_reached_goal() { + double diff_xyz = sqrt((target_position_.x - curr_position_.x) * + (target_position_.x - curr_position_.x) + + (target_position_.y - curr_position_.y) * + (target_position_.y - curr_position_.y) + + (target_position_.z - curr_position_.z) * + (target_position_.z - curr_position_.z)); + + double diff_yaw = + math_utils::angularDistance(target_position_.yaw, curr_position_.yaw); + + // todo save this in degrees somewhere to avoid repeated conversion + if (diff_xyz < params_.reached_thresh_xyz && + std::abs(diff_yaw) < math_utils::degToRad(params_.reached_yaw_degrees)) { + reached_goal_ = true; + } +} + +bool PIDPositionController::is_goal_reached() const { return reached_goal_; } + +void PIDPositionController::update_control_cmd_timer_cb( + const ros::TimerEvent& event) { + tick(); +} + +void PIDPositionController::tick() { + // todo check if odometry is too old!! + // if no odom, don't do anything. + if (!has_odom_) { + ROS_ERROR_STREAM("[PIDPositionController] Waiting for odometry!"); + return; + } + + if (has_goal_) { + check_reached_goal(); + if (reached_goal_) { + // ROS_INFO_STREAM("[PIDPositionController] Reached goal! Hovering at + // position."); + has_goal_ = false; + // dear future self, this function doesn't return coz we need to keep on + // actively hovering at last goal pose. don't act smart + } else { + // ROS_INFO_STREAM("[PIDPositionController] Moving to goal."); + } + } + + // only compute and send control commands for hovering / moving to pose, if we + // received a goal at least once in the past + if (got_goal_once_) { + compute_control_cmd(); + enforce_dynamic_constraints(); + publish_control_cmd(); + } +} + +void PIDPositionController::compute_control_cmd() { + curr_error_.x = target_position_.x - curr_position_.x; + curr_error_.y = target_position_.y - curr_position_.y; + curr_error_.z = target_position_.z - curr_position_.z; + curr_error_.yaw = + math_utils::angularDistance(curr_position_.yaw, target_position_.yaw); + + double p_term_x = params_.kp_x * curr_error_.x; + double p_term_y = params_.kp_y * curr_error_.y; + double p_term_z = params_.kp_z * curr_error_.z; + double p_term_yaw = params_.kp_yaw * curr_error_.yaw; + + double d_term_x = params_.kd_x * prev_error_.x; + double d_term_y = params_.kd_y * prev_error_.y; + double d_term_z = params_.kd_z * prev_error_.z; + double d_term_yaw = params_.kp_yaw * prev_error_.yaw; + + prev_error_ = curr_error_; + + vel_cmd_.linear.x = p_term_x + d_term_x; + vel_cmd_.linear.y = p_term_y + d_term_y; + vel_cmd_.linear.z = p_term_z + d_term_z; + vel_cmd_.angular.z = p_term_yaw + d_term_yaw; // todo +} + +void PIDPositionController::enforce_dynamic_constraints() { + double vel_norm_horz = sqrt((vel_cmd_.linear.x * vel_cmd_.linear.x) + + (vel_cmd_.linear.y * vel_cmd_.linear.y)); + + if (vel_norm_horz > constraints_.max_vel_horz_abs) { + vel_cmd_.linear.x = + (vel_cmd_.linear.x / vel_norm_horz) * constraints_.max_vel_horz_abs; + vel_cmd_.linear.y = + (vel_cmd_.linear.y / vel_norm_horz) * constraints_.max_vel_horz_abs; + } + + if (std::fabs(vel_cmd_.linear.z) > constraints_.max_vel_vert_abs) { + // todo just add a sgn funciton in common utils? return double to be safe. + // template double sgn(T val) { return (T(0) < val) - (val < + // T(0)); } + vel_cmd_.linear.z = (vel_cmd_.linear.z / std::fabs(vel_cmd_.linear.z)) * + constraints_.max_vel_vert_abs; + } + // todo yaw limits + if (std::fabs(vel_cmd_.linear.z) > constraints_.max_yaw_rate_degree) { + // todo just add a sgn funciton in common utils? return double to be safe. + // template double sgn(T val) { return (T(0) < val) - (val < + // T(0)); } + vel_cmd_.linear.z = (vel_cmd_.linear.z / std::fabs(vel_cmd_.linear.z)) * + constraints_.max_yaw_rate_degree; + } +} + +void PIDPositionController::publish_control_cmd() { + airsim_vel_cmd_world_frame_pub_.publish(vel_cmd_); +} + +bool PIDPositionController::get_velocity(geometry_msgs::Twist& vel) const { + if (got_goal_once_) { + vel = vel_cmd_; + return true; + } + return false; +} + +msr::airlib::YawMode PIDPositionController::getYawMode() const { + msr::airlib::YawMode yaw_mode; + yaw_mode.is_rate = true; + yaw_mode.yaw_or_rate = math_utils::radToDeg(vel_cmd_.angular.z); + return yaw_mode; +} + +} // namespace unreal_airsim From ae141b565cd712fc560ecb5108acf2c882f6cf19 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 17 Jan 2022 13:42:45 +0100 Subject: [PATCH 07/16] Formatting --- src/online_simulator/simulator.cpp | 57 +++++++++---------- .../infrared_id_compensation.cpp | 24 +++++--- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 622abd2..281b05d 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -19,8 +19,8 @@ STRICT_MODE_ON #include #include #include -#include #include +#include #include @@ -320,10 +320,10 @@ bool AirsimSimulator::setupROS() { // This assumes the camera exists, which should always be the case with // the auto-generated-config. camera->camera_info = airsim_move_client_.simGetCameraInfo( - camera->name, config_.vehicle_name); + camera->name, config_.vehicle_name); // TODO(Schmluk): Might want to also publish the camera info or convert // to intrinsics etc - } + } if (!config_.publish_sensor_transforms) { // Broadcast all sensor mounting transforms via static tf. @@ -423,10 +423,9 @@ void AirsimSimulator::commandTrajectorycallback( if (!is_running_) { return; } - + for (const auto& point : trajectory.points) { for (auto pose : point.transforms) { - frame_converter_.rosToAirsim(&pose); auto* pt = new Waypoint; @@ -448,7 +447,6 @@ void AirsimSimulator::commandTrajectorycallback( pt->pose.rotation.w = lastPose->pose.rotation.w; pt->isGoal = true; points.push(pt); - } void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { @@ -468,43 +466,43 @@ void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { // Use position + yaw as setpoint auto command_pos = T_gt_command.getPosition(); - + auto command_ori = T_gt_command.getEigenQuaternion(); - std::cout << "Pose Orientation In ROS Frame: " - << command_ori.x() << " " - << command_ori.y() << " " - << command_ori.z() << " " + std::cout << "Pose Orientation In ROS Frame: " << command_ori.x() << " " + << command_ori.y() << " " << command_ori.z() << " " << command_ori.w() << std::endl; frame_converter_.rosToAirsim(&command_ori); - std::cout << "Pose Orientation In Airsim Frame: " - << command_ori.x() << " " - << command_ori.y() << " " - << command_ori.z() << " " + std::cout << "Pose Orientation In Airsim Frame: " << command_ori.x() << " " + << command_ori.y() << " " << command_ori.z() << " " << command_ori.w() << std::endl; double yaw = tf2::getYaw( tf2::Quaternion(command_ori.x(), command_ori.y(), command_ori.z(), command_ori.w())); // Eigen's eulerAngles apparently // messes up some wrap arounds or direcions and gets the wrong yaws in some // cases - //config_.velocity =0.2f; + // config_.velocity =0.2f; yaw = yaw / M_PI * 180.0; constexpr double kMinMovingDistance = 0.1; // m airsim_move_client_.cancelLastTask(); if ((command_pos - t_gt_current_position).norm() >= kMinMovingDistance) { frame_converter_.rosToAirsim(&command_pos); auto yaw_mode = msr::airlib::YawMode(false, yaw); - float look_ahead=-1, adaptive_lookahead=0; + float look_ahead = -1, adaptive_lookahead = 0; airsim_move_client_.moveToPositionAsync( command_pos.x(), command_pos.y(), command_pos.z(), config_.velocity, - 3600, config_.drive_train_type, yaw_mode, look_ahead,adaptive_lookahead, config_.vehicle_name); - std::cout <<"Moving to target position with "<width == 0 || msg->height == 0) { + return; + } + cv_bridge::CvImagePtr img; try { - cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, msg->encoding); - for (int v = 0; v < img->image.rows; v++) { - for (int u = 0; u < img->image.cols; u++) { - img->image.at(v, u) = - infrared_compensation_[img->image.at(v, u)]; - } - } - pub_.publish(img->toImageMsg()); + img = cv_bridge::toCvCopy(msg, msg->encoding); } catch (const cv_bridge::Exception& e) { - LOG(WARNING) << "Could not convert image to CvImage (" << e.what() << "), skipping frame."; + LOG(WARNING) << "Could not convert image to CvImage (" << e.what() + << "), skipping frame."; + return; + } + for (int v = 0; v < img->image.rows; v++) { + for (int u = 0; u < img->image.cols; u++) { + img->image.at(v, u) = + infrared_compensation_[img->image.at(v, u)]; + } } + pub_.publish(img->toImageMsg()); } } // namespace unreal_airsim::simulator_processor From 7e7970e1e6db5b50df75aeb8ba9c0be695a91bba Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Tue, 18 Jan 2022 13:20:43 +0100 Subject: [PATCH 08/16] rapid prototyping for acd --- src/online_simulator/simulator.cpp | 2 +- src/simulator_processing/pid_controller.cpp | 8 -------- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 281b05d..7aea462 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -40,7 +40,7 @@ AirsimSimulator::AirsimSimulator(const ros::NodeHandle& nh, is_running_(false), is_shutdown_(false), followingGoal(false), - only_move_in_yaw_direction(true), + only_move_in_yaw_direction(false), goal_yaw(0), has_goal_(false), pid_controller_(nh, nh_private), diff --git a/src/simulator_processing/pid_controller.cpp b/src/simulator_processing/pid_controller.cpp index af0c706..f622d1a 100644 --- a/src/simulator_processing/pid_controller.cpp +++ b/src/simulator_processing/pid_controller.cpp @@ -94,8 +94,6 @@ bool PIDPositionController::set_target(const double x, const double y, } // Already have goal, and have reached it - // ROS_INFO_STREAM("[PIDPositionController] Already have goal and have reached - // it"); return false; } @@ -191,17 +189,11 @@ void PIDPositionController::enforce_dynamic_constraints() { } if (std::fabs(vel_cmd_.linear.z) > constraints_.max_vel_vert_abs) { - // todo just add a sgn funciton in common utils? return double to be safe. - // template double sgn(T val) { return (T(0) < val) - (val < - // T(0)); } vel_cmd_.linear.z = (vel_cmd_.linear.z / std::fabs(vel_cmd_.linear.z)) * constraints_.max_vel_vert_abs; } // todo yaw limits if (std::fabs(vel_cmd_.linear.z) > constraints_.max_yaw_rate_degree) { - // todo just add a sgn funciton in common utils? return double to be safe. - // template double sgn(T val) { return (T(0) < val) - (val < - // T(0)); } vel_cmd_.linear.z = (vel_cmd_.linear.z / std::fabs(vel_cmd_.linear.z)) * constraints_.max_yaw_rate_degree; } From 03365d0138cc1a1ca70e23324f10cf0bdc48aa15 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 14 Mar 2022 13:56:19 +0100 Subject: [PATCH 09/16] re-organize PID parameter reading --- src/simulator_processing/pid_controller.cpp | 52 +++++++++++++++------ 1 file changed, 39 insertions(+), 13 deletions(-) diff --git a/src/simulator_processing/pid_controller.cpp b/src/simulator_processing/pid_controller.cpp index f622d1a..4ad7691 100644 --- a/src/simulator_processing/pid_controller.cpp +++ b/src/simulator_processing/pid_controller.cpp @@ -5,18 +5,38 @@ namespace unreal_airsim { bool PIDParams::load_from_rosparams(const ros::NodeHandle& nh) { bool found = true; - found = nh.getParam("kp_x", kp_x) && found; - found = nh.getParam("kp_y", kp_y) && found; - found = nh.getParam("kp_z", kp_z) && found; - found = nh.getParam("kp_yaw", kp_yaw) && found; + if (!nh.getParam("kp_x", kp_x)) { + found = false; + } + if (!nh.getParam("kp_y", kp_y)) { + found = false; + } + if (!nh.getParam("kp_z", kp_z)) { + found = false; + } + if (!nh.getParam("kp_yaw", kp_yaw)) { + found = false; + } - found = nh.getParam("kd_x", kd_x) && found; - found = nh.getParam("kd_y", kd_y) && found; - found = nh.getParam("kd_z", kd_z) && found; - found = nh.getParam("kd_yaw", kd_yaw) && found; + if (!nh.getParam("kd_x", kd_x)) { + found = false; + } + if (!nh.getParam("kd_y", kd_y)) { + found = false; + } + if (!nh.getParam("kd_z", kd_z)) { + found = false; + } + if (!nh.getParam("kd_yaw", kd_yaw)) { + found = false; + } - found = nh.getParam("reached_thresh_xyz", reached_yaw_degrees) && found; - found = nh.getParam("reached_yaw_degrees", reached_yaw_degrees) && found; + if (!nh.getParam("reached_thresh_xyz", reached_yaw_degrees)) { + found = false; + } + if (!nh.getParam("reached_yaw_degrees", reached_yaw_degrees)) { + found = false; + } return found; } @@ -24,9 +44,15 @@ bool PIDParams::load_from_rosparams(const ros::NodeHandle& nh) { bool DynamicConstraints::load_from_rosparams(const ros::NodeHandle& nh) { bool found = true; - found = nh.getParam("max_vel_horz_abs", max_vel_horz_abs) && found; - found = nh.getParam("max_vel_vert_abs", max_vel_vert_abs) && found; - found = nh.getParam("max_yaw_rate_degree", max_yaw_rate_degree) && found; + if (!nh.getParam("max_vel_horz_abs", max_vel_horz_abs)) { + found = false; + } + if (!nh.getParam("max_vel_vert_abs", max_vel_vert_abs)) { + found = false; + } + if (!nh.getParam("max_yaw_rate_degree", max_yaw_rate_degree)) { + found = false; + } return found; } From f92fc003e05be325ad14cd99ad95bad02a808cd4 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 14 Mar 2022 14:06:31 +0100 Subject: [PATCH 10/16] fix yaw limits for rotational speed --- src/simulator_processing/pid_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulator_processing/pid_controller.cpp b/src/simulator_processing/pid_controller.cpp index 4ad7691..0d4e074 100644 --- a/src/simulator_processing/pid_controller.cpp +++ b/src/simulator_processing/pid_controller.cpp @@ -219,8 +219,8 @@ void PIDPositionController::enforce_dynamic_constraints() { constraints_.max_vel_vert_abs; } // todo yaw limits - if (std::fabs(vel_cmd_.linear.z) > constraints_.max_yaw_rate_degree) { - vel_cmd_.linear.z = (vel_cmd_.linear.z / std::fabs(vel_cmd_.linear.z)) * + if (std::fabs(vel_cmd_.angular.z) > constraints_.max_yaw_rate_degree) { + vel_cmd_.angular.z = (vel_cmd_.angular.z / std::fabs(vel_cmd_.angular.z)) * constraints_.max_yaw_rate_degree; } } From 90a83d296d69c0c6d2fc9b5332428e27873ffa06 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 14 Mar 2022 14:13:05 +0100 Subject: [PATCH 11/16] handle empty depth images in depth to pointcloud --- src/simulator_processing/depth_to_pointcloud.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/simulator_processing/depth_to_pointcloud.cpp b/src/simulator_processing/depth_to_pointcloud.cpp index 0dd4311..1fb0932 100644 --- a/src/simulator_processing/depth_to_pointcloud.cpp +++ b/src/simulator_processing/depth_to_pointcloud.cpp @@ -221,6 +221,11 @@ void DepthToPointcloud::publishPointcloud( if (pub_.getNumSubscribers() == 0) { return; } + if (depth_ptr->height == 0) { + LOG(WARNING) << "Received empty depth image, skipping frame."; + return; + } + cv_bridge::CvImageConstPtr depth_img, color_img, segmentation_img; try { depth_img = cv_bridge::toCvShare(depth_ptr, depth_ptr->encoding); From da4795d2458ba34c02edfba990ae4376b13d360a Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 14 Mar 2022 15:21:12 +0100 Subject: [PATCH 12/16] fix pid rate saturation bugs pt 1 --- src/simulator_processing/pid_controller.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/simulator_processing/pid_controller.cpp b/src/simulator_processing/pid_controller.cpp index 0d4e074..40e7ab3 100644 --- a/src/simulator_processing/pid_controller.cpp +++ b/src/simulator_processing/pid_controller.cpp @@ -66,7 +66,7 @@ PIDPositionController::PIDPositionController(const ros::NodeHandle& nh, reached_goal_(false), got_goal_once_(false) { params_.load_from_rosparams(nh_private_); - constraints_.load_from_rosparams(nh_); + constraints_.load_from_rosparams(nh_private_); initialize_ros(); reset_errors(); } @@ -219,9 +219,11 @@ void PIDPositionController::enforce_dynamic_constraints() { constraints_.max_vel_vert_abs; } // todo yaw limits - if (std::fabs(vel_cmd_.angular.z) > constraints_.max_yaw_rate_degree) { - vel_cmd_.angular.z = (vel_cmd_.angular.z / std::fabs(vel_cmd_.angular.z)) * - constraints_.max_yaw_rate_degree; + const double max_yaw_rate_rad = + constraints_.max_yaw_rate_degree / 180.0 * M_PI; + if (std::fabs(vel_cmd_.angular.z) > max_yaw_rate_rad) { + vel_cmd_.angular.z = + (vel_cmd_.angular.z / std::fabs(vel_cmd_.angular.z)) * max_yaw_rate_rad; } } From 78dd831e926b5feb014d129aed439fcb8a10269e Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Tue, 15 Mar 2022 10:38:39 +0100 Subject: [PATCH 13/16] fix crash on reset --- src/online_simulator/simulator.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 7aea462..ab128ee 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -535,7 +535,7 @@ bool AirsimSimulator::startSimTimer() { LOG(INFO) << "Using sim time: AirSim time will be published as ROS time by " "the simulator."; std::thread([this]() { - while (is_connected_) { + while (is_connected_ && !is_shutdown_) { auto next = std::chrono::steady_clock::now() + std::chrono::milliseconds(config_.time_publisher_interval); readSimTimeCallback(); @@ -808,8 +808,11 @@ void AirsimSimulator::onShutdown() { for (const auto& timer : sensor_timers_) { timer->signalShutdown(); } + sim_state_timer_ = ros::Timer(); if (is_connected_) { - LOG(INFO) << "Shutting down: resetting airsim server."; + LOG(INFO) << "Shutting down: resetting airsim server in 10 seconds."; + sleep(10); + LOG(INFO) << "Shutting down: Resetting airsim server."; airsim_state_client_.reset(); airsim_state_client_.enableApiControl(false); } From 545afe64beb1f3fd509ab43bd7b743d6e5c9514d Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Tue, 15 Mar 2022 10:43:08 +0100 Subject: [PATCH 14/16] make reset timer a param --- include/unreal_airsim/online_simulator/simulator.h | 4 ++++ src/online_simulator/simulator.cpp | 12 ++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/include/unreal_airsim/online_simulator/simulator.h b/include/unreal_airsim/online_simulator/simulator.h index 7802e8c..0dbd6b6 100644 --- a/include/unreal_airsim/online_simulator/simulator.h +++ b/include/unreal_airsim/online_simulator/simulator.h @@ -62,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"; @@ -83,6 +85,7 @@ 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; @@ -90,6 +93,7 @@ class AirsimSimulator { msr::airlib::ImageCaptureBase::ImageType::Scene; msr::airlib::CameraInfo camera_info; // The info is read from UE4 }; + std::vector> sensors; }; diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index ab128ee..642c5ca 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -87,8 +87,10 @@ bool AirsimSimulator::readParamsFromRos() { nh_private_.param("velocity", config_.velocity, defaults.velocity); nh_private_.param("publish_sensor_transforms", config_.publish_sensor_transforms, - defaults.publish_sensor_transforms); - + defaults.publish_sensor_transforms);rest_timer +nh_private_.param("reset_timer", + config_.reset_timer, + defaults.reset_timer); // Verify params valid if (config_.state_refresh_rate <= 0.0) { config_.state_refresh_rate = defaults.state_refresh_rate; @@ -810,8 +812,10 @@ void AirsimSimulator::onShutdown() { } sim_state_timer_ = ros::Timer(); if (is_connected_) { - LOG(INFO) << "Shutting down: resetting airsim server in 10 seconds."; - sleep(10); + if (config_.reset_timer > 0.f) { + LOG(INFO) << "Shutting down: resetting airsim server in "<< config_.reset_timer<<" seconds."; + sleep(config_.reset_timer); + } LOG(INFO) << "Shutting down: Resetting airsim server."; airsim_state_client_.reset(); airsim_state_client_.enableApiControl(false); From 4f5404bb5d2aa80e3ad7c6c3859d495cce1ba613 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Tue, 15 Mar 2022 10:57:55 +0100 Subject: [PATCH 15/16] fix typo --- src/online_simulator/simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 642c5ca..666f4c3 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -87,7 +87,7 @@ bool AirsimSimulator::readParamsFromRos() { nh_private_.param("velocity", config_.velocity, defaults.velocity); nh_private_.param("publish_sensor_transforms", config_.publish_sensor_transforms, - defaults.publish_sensor_transforms);rest_timer + defaults.publish_sensor_transforms); nh_private_.param("reset_timer", config_.reset_timer, defaults.reset_timer); From 21a6144c913686b8a698b54a583fd0e82ceebaf9 Mon Sep 17 00:00:00 2001 From: Lukas Schmid Date: Mon, 4 Apr 2022 14:08:59 +0200 Subject: [PATCH 16/16] fix rotation for python3 --- app/tools/parse_config_to_airsim.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/app/tools/parse_config_to_airsim.py b/app/tools/parse_config_to_airsim.py index b3e324e..f9996d9 100755 --- a/app/tools/parse_config_to_airsim.py +++ b/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