diff --git a/README.md b/README.md index db1e3ac..6966565 100644 --- a/README.md +++ b/README.md @@ -149,4 +149,17 @@ For more see the following image: # Known Issues * No ROS communication between docker container and host -* Mesh in rviz does not rotate with drone \ No newline at end of file + + + + +# Projects using this repository + +- [Drona🤖✈️](https://github.com/Gaurang-1402/Drona): is a drone control software that enables drones to be operated using Large Language Models, emphasizing ease of use and accessibility. It's designed to interact with real-world scenarios, specifically in fields like agriculture and disaster relief, where drones can be used for tasks like monitoring crop health or aiding in search and rescue operations, all controlled through simplified, multilingual commands. +- [Window Washing Drone](https://github.com/ayushchakra/window-washing-drone): is a project that aims to automate the process of window washing using a drone. +- [ChatDrones](https://github.com/Gaurang-1402/ChatDrones): is a project that merges Large Language Models with drone control, enabling users to operate drones through simple natural language commands. It includes a user-friendly web application, allowing for easy input of commands in multiple languages and control of drone movements such as takeoff, landing, and directional navigation. + + + + + diff --git a/sjtu_drone_bringup/launch/sjtu_drone_gazebo.launch.py b/sjtu_drone_bringup/launch/sjtu_drone_gazebo.launch.py index dc7270b..3934ae5 100755 --- a/sjtu_drone_bringup/launch/sjtu_drone_gazebo.launch.py +++ b/sjtu_drone_bringup/launch/sjtu_drone_gazebo.launch.py @@ -102,5 +102,12 @@ def launch_gzclient(context, *args, **kwargs): executable="spawn_drone", arguments=[robot_desc, model_ns], output="screen" - ) + ), + + Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "world", f"{model_ns}/odom"], + output="screen" + ), ]) diff --git a/sjtu_drone_description/CMakeLists.txt b/sjtu_drone_description/CMakeLists.txt index 83382cb..b6407f7 100644 --- a/sjtu_drone_description/CMakeLists.txt +++ b/sjtu_drone_description/CMakeLists.txt @@ -21,6 +21,10 @@ find_package(gazebo_dev REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) link_directories(${GAZEBO_LIBRARY_DIRS} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} ${OGRE_LIBRARY_DIRS}) @@ -37,9 +41,11 @@ include_directories(${OpenCV_INCLUDE_DIR} ################## 2. A simple model controller for the quadrotor ############# add_library( plugin_drone SHARED src/plugin_drone.cpp +src/plugin_drone_private.cpp src/pid_controller.cpp -include/plugin_drone.h -include/pid_controller.h +include/sjtu_drone_description/plugin_drone.h +include/sjtu_drone_description/plugin_drone_private.h +include/sjtu_drone_description/pid_controller.h ) @@ -55,6 +61,10 @@ ament_target_dependencies(plugin_drone "std_msgs" "geometry_msgs" "sensor_msgs" + "nav_msgs" + "tf2" + "tf2_ros" + "tf2_geometry_msgs" ) diff --git a/sjtu_drone_description/include/pid_controller.h b/sjtu_drone_description/include/sjtu_drone_description/pid_controller.h similarity index 88% rename from sjtu_drone_description/include/pid_controller.h rename to sjtu_drone_description/include/sjtu_drone_description/pid_controller.h index d14bca5..4eeb549 100644 --- a/sjtu_drone_description/include/pid_controller.h +++ b/sjtu_drone_description/include/sjtu_drone_description/pid_controller.h @@ -17,13 +17,13 @@ #include -#include -class PIDController { +class PIDController +{ public: PIDController(); virtual ~PIDController(); - virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = ""); + virtual void Load(sdf::ElementPtr _sdf, const std::string & prefix = ""); double gain_p; double gain_i; diff --git a/sjtu_drone_description/include/plugin_drone.h b/sjtu_drone_description/include/sjtu_drone_description/plugin_drone.h similarity index 60% rename from sjtu_drone_description/include/plugin_drone.h rename to sjtu_drone_description/include/sjtu_drone_description/plugin_drone.h index 72aa974..10f7459 100644 --- a/sjtu_drone_description/include/plugin_drone.h +++ b/sjtu_drone_description/include/sjtu_drone_description/plugin_drone.h @@ -12,50 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLUGIN_DRONE_H -#define PLUGIN_DRONE_H +#ifndef GAZEBO_PLUGINS_DRONE_SIMPLE_H +#define GAZEBO_PLUGINS_DRONE_SIMPLE_H -#include "gazebo_ros/node.hpp" -#include "gazebo/gazebo.hh" -#include "gazebo/physics/physics.hh" -#include "gazebo/common/Events.hh" +#include "gazebo/physics/Link.hh" +#include "gazebo/physics/Model.hh" +#include "gazebo/physics/World.hh" +#include "sdf/sdf.hh" -#include -#include +#include "sjtu_drone_description/plugin_drone_private.h" -#include -#include -#include -#include -#include -#include -#include - -#include "pid_controller.h" - -#define LANDED_MODEL 0 -#define FLYING_MODEL 1 -#define TAKINGOFF_MODEL 2 -#define LANDING_MODEL 3 using namespace std::placeholders; namespace gazebo_plugins { -// Forward declaration of pimpl idiom -class DroneSimpleControllerPrivate; - class DroneSimpleController : public gazebo::ModelPlugin { public: - DroneSimpleController(); - virtual ~DroneSimpleController(); + DroneSimpleController(void); + virtual ~DroneSimpleController(void); protected: virtual void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Update(); - virtual void Reset(); + virtual void Update(const gazebo::common::UpdateInfo & _info); + virtual void Reset(void); private: std::unique_ptr impl_; // Forward declaration of pimpl idiom @@ -75,4 +57,4 @@ class DroneSimpleController : public gazebo::ModelPlugin } -#endif // PLUGIN_DRONE_HPP \ No newline at end of file +#endif // GAZEBO_PLUGINS_DRONE_SIMPLE_H diff --git a/sjtu_drone_description/include/sjtu_drone_description/plugin_drone_private.h b/sjtu_drone_description/include/sjtu_drone_description/plugin_drone_private.h new file mode 100644 index 0000000..37a5397 --- /dev/null +++ b/sjtu_drone_description/include/sjtu_drone_description/plugin_drone_private.h @@ -0,0 +1,171 @@ +// Copyright 2024 Georg Novotny +// +// Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.gnu.org/licenses/gpl-3.0.en.html +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS_DRONE_SIMPLE_PRIVATE_H +#define GAZEBO_PLUGINS_DRONE_SIMPLE_PRIVATE_H + +#include "gazebo_ros/node.hpp" +#include "gazebo/physics/Link.hh" +#include "gazebo/physics/Model.hh" +#include "gazebo/physics/World.hh" +#include "sdf/sdf.hh" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "sjtu_drone_description/pid_controller.h" + + +#define LANDED_MODEL 0 +#define FLYING_MODEL 1 +#define TAKINGOFF_MODEL 2 +#define LANDING_MODEL 3 + +namespace gazebo_plugins +{ + +class DroneSimpleControllerPrivate +{ +public: + DroneSimpleControllerPrivate(void); + ~DroneSimpleControllerPrivate(void); + + void Reset(); + void InitSubscribers( + std::string cmd_normal_topic_ = "cmd_vel", std::string posctrl_topic_ = "posctrl", + std::string imu_topic_ = "imu", std::string takeoff_topic_ = "takeoff", + std::string land_topic_ = "land", std::string reset_topic_ = "reset", + std::string switch_mode_topic_ = "dronevel_mode"); + void InitPublishers( + std::string gt_topic_ = "gt_pose", std::string gt_vel_topic_ = "gt_vel", + std::string gt_acc_topic_ = "gt_acc", std::string cmd_mode_topic_ = "cmd_mode", + std::string state_topic_ = "state", std::string odom_topic_ = "odom"); + void LoadControllerSettings(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf); + + void UpdateState(double dt); + void UpdateDynamics(double dt); + + // Gazebo variables + gazebo::physics::WorldPtr world; + gazebo::physics::LinkPtr link; + gazebo::physics::ModelPtr model; + gazebo::common::Time current_time; + gazebo::common::Time last_time; + + // Simunlation configuration + double max_force_; + double motion_small_noise_; + double motion_drift_noise_; + double motion_drift_noise_time_; + + // Inertia and mass of the drone + ignition::math::v6::Vector3 inertia; + double mass; + double m_timeAfterCmd; + int navi_state; + bool m_posCtrl; + bool m_velMode; + int odom_seq; + int odom_hz; + bool pub_odom; + + // rclcpp configuration + rclcpp::Node::SharedPtr ros_node_{nullptr}; + rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; + std::unique_ptr tf_broadcaster_; + +private: + rclcpp::SubscriptionOptions create_subscription_options( + std::string topic_name, + uint32_t queue_size); + + // Callbacks + void CmdCallback(const geometry_msgs::msg::Twist::SharedPtr cmd); + void PosCtrlCallback(const std_msgs::msg::Bool::SharedPtr cmd); + void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu); + void TakeoffCallback(const std_msgs::msg::Empty::SharedPtr msg); + void LandCallback(const std_msgs::msg::Empty::SharedPtr msg); + void ResetCallback(const std_msgs::msg::Empty::SharedPtr msg); + void SwitchModeCallback(const std_msgs::msg::Bool::SharedPtr msg); + + double last_odom_publish_time_; + void PublishOdom( + const ignition::math::v6::Pose3 & pose, + const ignition::math::v6::Vector3 & velocity, + const ignition::math::v6::Vector3 & acceleration); + + struct Controllers + { + PIDController roll; + PIDController pitch; + PIDController yaw; + PIDController velocity_x; + PIDController velocity_y; + PIDController velocity_z; + PIDController pos_x; + PIDController pos_y; + PIDController pos_z; + }; + + // ROS Interfaces + geometry_msgs::msg::Twist cmd_vel; + nav_msgs::msg::Odometry odom; + + + // Subscribers + rclcpp::Subscription::SharedPtr cmd_subscriber_{nullptr}; + rclcpp::Subscription::SharedPtr posctrl_subscriber_{nullptr}; + rclcpp::Subscription::SharedPtr imu_subscriber_{nullptr}; + rclcpp::Subscription::SharedPtr takeoff_subscriber_{nullptr}; + rclcpp::Subscription::SharedPtr land_subscriber_{nullptr}; + rclcpp::Subscription::SharedPtr reset_subscriber_{nullptr}; + rclcpp::Subscription::SharedPtr switch_mode_subscriber_{nullptr}; + + // Publishers + rclcpp::Publisher::SharedPtr pub_gt_pose_{nullptr}; //for publishing ground truth pose + rclcpp::Publisher::SharedPtr pub_gt_vec_{nullptr}; //ground truth velocity in the body frame + rclcpp::Publisher::SharedPtr pub_gt_acc_{nullptr}; //ground truth acceleration in the body frame + rclcpp::Publisher::SharedPtr pub_cmd_mode{nullptr}; //for publishing command mode + rclcpp::Publisher::SharedPtr pub_state{nullptr}; //for publishing current STATE (Landed, Flying, Takingoff, Landing) + rclcpp::Publisher::SharedPtr pub_odom_{nullptr}; //for publishing odometry + + // PID Controller + Controllers controllers_; + + // State of the drone + ignition::math::v6::Pose3 pose; + ignition::math::v6::Vector3 euler; + ignition::math::v6::Vector3 velocity, acceleration, angular_velocity, position; + + +}; // class DroneSimpleControllerPrivate + +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS_DRONE_SIMPLE_PRIVATE_H diff --git a/sjtu_drone_description/src/.vscode/c_cpp_properties.json b/sjtu_drone_description/src/.vscode/c_cpp_properties.json deleted file mode 100644 index 23578a6..0000000 --- a/sjtu_drone_description/src/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,22 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "", - "limitSymbolsToIncludedHeaders": true - }, - "includePath": [ - "/home/georg/catkin_ws/devel/include/**", - "/opt/ros/noetic/include/**", - "/home/georg/catkin_ws/src/LMD/follow_human/include/**", - "/home/georg/catkin_ws/src/LMD/LIO-SAM/include/**", - "/home/georg/catkin_ws/src/LMD/robot_control/include/**", - "/home/georg/catkin_ws/src/LMD/robot_simulation/include/**", - "/home/georg/catkin_ws/src/rl_course_files/parrot_ardrone/sjtu_drone/include/**", - "/usr/include/**" - ], - "name": "ROS" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/sjtu_drone_description/src/.vscode/settings.json b/sjtu_drone_description/src/.vscode/settings.json deleted file mode 100644 index 13f7cae..0000000 --- a/sjtu_drone_description/src/.vscode/settings.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/home/georg/catkin_ws/devel/lib/python3/dist-packages", - "/opt/ros/noetic/lib/python3/dist-packages" - ], - "python.analysis.extraPaths": [ - "/home/georg/catkin_ws/devel/lib/python3/dist-packages", - "/opt/ros/noetic/lib/python3/dist-packages" - ] -} \ No newline at end of file diff --git a/sjtu_drone_description/src/pid_controller.cpp b/sjtu_drone_description/src/pid_controller.cpp index 0a82327..0122e85 100644 --- a/sjtu_drone_description/src/pid_controller.cpp +++ b/sjtu_drone_description/src/pid_controller.cpp @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_controller.h" +#include "sjtu_drone_description/pid_controller.h" PIDController::PIDController() { - + } -PIDController::~PIDController(){ - +PIDController::~PIDController() +{ + } -void PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix) +void PIDController::Load(sdf::ElementPtr _sdf, const std::string & prefix) { gain_p = 5.0; gain_d = 1.0; @@ -32,22 +33,32 @@ void PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix) time_constant = 0.0; limit = -1.0; - if (!_sdf) return; - if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->Get(); - if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->Get(); - if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->Get(); - if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->Get(); - if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->Get(); + if (!_sdf) {return;} + if (_sdf->HasElement(prefix + "ProportionalGain")) { + gain_p = _sdf->GetElement(prefix + "ProportionalGain")->Get(); + } + if (_sdf->HasElement(prefix + "DifferentialGain")) { + gain_d = _sdf->GetElement(prefix + "DifferentialGain")->Get(); + } + if (_sdf->HasElement(prefix + "IntegralGain")) { + gain_i = _sdf->GetElement(prefix + "IntegralGain")->Get(); + } + if (_sdf->HasElement(prefix + "TimeConstant")) { + time_constant = _sdf->GetElement(prefix + "TimeConstant")->Get(); + } + if (_sdf->HasElement(prefix + "Limit")) { + limit = _sdf->GetElement(prefix + "Limit")->Get(); + } } double PIDController::update(double new_input, double x, double dx, double dt) { // limit command - if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit; + if (limit > 0.0 && fabs(new_input) > limit) {new_input = (new_input < 0 ? -1.0 : 1.0) * limit;} // filter command if (dt + time_constant > 0.0) { - input = (dt * new_input + time_constant * input) / (dt + time_constant); + input = (dt * new_input + time_constant * input) / (dt + time_constant); dinput = (new_input - input) / (dt + time_constant); } @@ -66,4 +77,3 @@ void PIDController::reset() input = dinput = 0; p = i = d = output = 0; } - diff --git a/sjtu_drone_description/src/plugin_drone.cpp b/sjtu_drone_description/src/plugin_drone.cpp index 02cb791..05dbc71 100644 --- a/sjtu_drone_description/src/plugin_drone.cpp +++ b/sjtu_drone_description/src/plugin_drone.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "plugin_drone.h" +#include "sjtu_drone_description/plugin_drone.h" #include @@ -20,548 +20,8 @@ #include -namespace gazebo_plugins { - -class DroneSimpleControllerPrivate { -public: - DroneSimpleControllerPrivate() - : m_timeAfterCmd(0.0) - , navi_state(LANDED_MODEL) - , m_posCtrl(false) - , m_velMode(false) - { - } - - ~DroneSimpleControllerPrivate() {} - - void Reset() - { - // Reset the values of the controllers - controllers_.roll.reset(); - controllers_.pitch.reset(); - controllers_.yaw.reset(); - controllers_.velocity_x.reset(); - controllers_.velocity_y.reset(); - controllers_.velocity_z.reset(); - - // Set the force and torque acting on the drone to zero - link->SetForce(ignition::math::Vector3(0.0, 0.0, 0.0)); - link->SetTorque(ignition::math::v6::Vector3(0.0, 0.0, 0.0)); - - // Reset the state of the drone - pose.Reset(); - velocity.Set(); - angular_velocity.Set(); - acceleration.Set(); - euler.Set(); - } - - // ROS Node configuration - void InitSubscribers(std::string cmd_normal_topic_, std::string posctrl_topic_, std::string imu_topic_, std::string takeoff_topic_, std::string land_topic_, std::string reset_topic_, std::string switch_mode_topic_) - { - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(); - if (!cmd_normal_topic_.empty()){ - auto sub_opt = create_subscription_options(cmd_normal_topic_, 1); - cmd_subscriber_ = ros_node_->create_subscription( - cmd_normal_topic_, qos, std::bind(&DroneSimpleControllerPrivate::CmdCallback, this, std::placeholders::_1), - sub_opt); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No cmd_topic defined!"); - - if (!posctrl_topic_.empty()) { - auto sub_opt = create_subscription_options(posctrl_topic_, 1); - posctrl_subscriber_ = ros_node_->create_subscription( - posctrl_topic_, qos, std::bind(&DroneSimpleControllerPrivate::PosCtrlCallback, this, std::placeholders::_1), - sub_opt); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No position control defined!"); - - // subscribe imu - if (!imu_topic_.empty()) { - auto sub_opt = create_subscription_options(imu_topic_, 1); - imu_subscriber_ = ros_node_->create_subscription( - imu_topic_, qos, std::bind(&DroneSimpleControllerPrivate::ImuCallback, this, std::placeholders::_1), - sub_opt); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No imu topic defined!"); - - // subscribe command: take off command - if (!takeoff_topic_.empty()) { - auto sub_opt = create_subscription_options(takeoff_topic_, 1); - takeoff_subscriber_ = ros_node_->create_subscription( - takeoff_topic_, qos, std::bind(&DroneSimpleControllerPrivate::TakeoffCallback, this, std::placeholders::_1), - sub_opt - ); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No takeoff topic defined!"); - - // subscribe command: land command - if (!land_topic_.empty()) { - auto sub_opt = create_subscription_options(land_topic_, 1); - land_subscriber_ = ros_node_->create_subscription( - land_topic_, qos, std::bind(&DroneSimpleControllerPrivate::LandCallback, this, std::placeholders::_1), - sub_opt); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No land topic defined!"); - - // subscribe command: reset command - if (!reset_topic_.empty()) { - auto sub_opt = create_subscription_options(reset_topic_, 1); - reset_subscriber_ = ros_node_->create_subscription( - reset_topic_, qos, std::bind(&DroneSimpleControllerPrivate::ResetCallback, this, std::placeholders::_1), - sub_opt); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No reset topic defined!"); - - // Subscribe command: switch mode command - if (!switch_mode_topic_.empty()) { - auto sub_opt = create_subscription_options(switch_mode_topic_, 1); - switch_mode_subscriber_ = ros_node_->create_subscription( - switch_mode_topic_, qos, std::bind(&DroneSimpleControllerPrivate::SwitchModeCallback, this, std::placeholders::_1), - sub_opt); - } - else - RCLCPP_ERROR(ros_node_->get_logger(), "No switch mode topic defined!"); - } - - void InitPublishers(std::string gt_topic_, std::string gt_vel_topic_, std::string gt_acc_topic_, std::string cmd_mode_topic_, std::string state_topic_) - { - if (!gt_topic_.empty()) - pub_gt_pose_ = ros_node_->create_publisher(gt_topic_,1024); - else - RCLCPP_ERROR(ros_node_->get_logger(), "No ground truth topic defined!"); - - if (!gt_vel_topic_.empty()) - pub_gt_vec_ = ros_node_->create_publisher("gt_vel", 1024); - else - RCLCPP_ERROR(ros_node_->get_logger(), "No ground truth velocity topic defined!"); - - if (!gt_acc_topic_.empty()) - pub_gt_acc_ = ros_node_->create_publisher("gt_acc", 1024); - else - RCLCPP_ERROR(ros_node_->get_logger(), "No ground truth acceleration topic defined!"); - - if (!cmd_mode_topic_.empty()) - pub_cmd_mode = ros_node_->create_publisher(cmd_mode_topic_, 1024); - else - RCLCPP_ERROR(ros_node_->get_logger(), "No command mode topic defined!"); - - if (!state_topic_.empty()) - pub_state = ros_node_->create_publisher(state_topic_, 1024); - else - RCLCPP_ERROR(ros_node_->get_logger(), "No state topic defined!"); - } - - - // Controller configuration - /** - * @brief Initiliaze the PID params - * - * @param _model shared pointer to the model object - * @param _sdf shared pointer to the sdf object - */ - void LoadControllerSettings(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf){ - controllers_.roll.Load(_sdf, "rollpitch"); - controllers_.pitch.Load(_sdf, "rollpitch"); - controllers_.yaw.Load(_sdf, "yaw"); - controllers_.velocity_x.Load(_sdf, "velocityXY"); - controllers_.velocity_y.Load(_sdf, "velocityXY"); - controllers_.velocity_z.Load(_sdf, "velocityZ"); - - controllers_.pos_x.Load(_sdf, "positionXY"); - controllers_.pos_y.Load(_sdf, "positionXY"); - controllers_.pos_z.Load(_sdf, "positionZ"); - - RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Using the PID parameters: \n" << - "\tRoll Pitch:\n" << "\t\tkP: " << controllers_.roll.gain_p << ", kI: " << controllers_.roll.gain_i << ",kD: " << controllers_.roll.gain_d << ", Limit: " << controllers_.roll.limit << ", Time Constant: " << controllers_.roll.time_constant << "\n" << - "\tYaw:\n" << "\t\tkP: " << controllers_.yaw.gain_p << ", kI: " << controllers_.yaw.gain_i << ",kD: " << controllers_.yaw.gain_d << ", Limit: " << controllers_.yaw.limit << ", Time Constant: " << controllers_.yaw.time_constant << "\n" << - "\tVelocity X:\n" << "\t\tkP: " << controllers_.velocity_x.gain_p << ", kI: " << controllers_.velocity_x.gain_i << ",kD: " << controllers_.velocity_x.gain_d << ", Limit: " << controllers_.velocity_x.limit << ", Time Constant: " << controllers_.velocity_x.time_constant << "\n" << - "\tVelocity Y:\n" << "\t\tkP: " << controllers_.velocity_y.gain_p << ", kI: " << controllers_.velocity_y.gain_i << ",kD: " << controllers_.velocity_y.gain_d << ", Limit: " << controllers_.velocity_y.limit << ", Time Constant: " << controllers_.velocity_y.time_constant << "\n" << - "\tVelocity Z:\n" << "\t\tkP: " << controllers_.velocity_z.gain_p << ", kI: " << controllers_.velocity_z.gain_i << ",kD: " << controllers_.velocity_z.gain_d << ", Limit: " << controllers_.velocity_z.limit << ", Time Constant: " << controllers_.velocity_z.time_constant << "\n" << - "\tPosition XY:\n" << "\t\tkP: " << controllers_.pos_x.gain_p << ", kI: " << controllers_.pos_x.gain_i << ",kD: " << controllers_.pos_x.gain_d << ", Limit: " << controllers_.pos_x.limit << ", Time Constant: " << controllers_.pos_x.time_constant << "\n" << - "\tPosition Z:\n" << "\t\tkP: " << controllers_.pos_z.gain_p << ", kI: " << controllers_.pos_z.gain_i << ",kD: " << controllers_.pos_z.gain_d << ", Limit: " << controllers_.pos_z.limit << ", Time Constant: " << controllers_.pos_z.time_constant - ); - } - - - struct Controllers { - PIDController roll; - PIDController pitch; - PIDController yaw; - PIDController velocity_x; - PIDController velocity_y; - PIDController velocity_z; - PIDController pos_x; - PIDController pos_y; - PIDController pos_z; - }; - - // Gazebo variables - gazebo::physics::WorldPtr world; - gazebo::physics::LinkPtr link; - - // Simunlation configuration - double max_force_; - double motion_small_noise_; - double motion_drift_noise_; - double motion_drift_noise_time_; - - // Inertia and mass of the drone - ignition::math::v6::Vector3 inertia; - double mass; - - -// private: -// friend class DroneSimpleController; // Add explicit friend declaration to access private data members - - // Plugin configuration - double m_timeAfterCmd; - int navi_state; - bool m_posCtrl; - bool m_velMode; - - // rclcpp configuration - rclcpp::Node::SharedPtr ros_node_{nullptr}; - rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; - - // ROS Interfaces - geometry_msgs::msg::Twist cmd_vel; - - // Subscribers - rclcpp::Subscription::SharedPtr cmd_subscriber_{nullptr}; - rclcpp::Subscription::SharedPtr posctrl_subscriber_{nullptr}; - rclcpp::Subscription::SharedPtr imu_subscriber_{nullptr}; - rclcpp::Subscription::SharedPtr takeoff_subscriber_{nullptr}; - rclcpp::Subscription::SharedPtr land_subscriber_{nullptr}; - rclcpp::Subscription::SharedPtr reset_subscriber_{nullptr}; - rclcpp::Subscription::SharedPtr switch_mode_subscriber_{nullptr}; - - // Publishers - rclcpp::Publisher::SharedPtr pub_gt_pose_{nullptr}; //for publishing ground truth pose - rclcpp::Publisher::SharedPtr pub_gt_vec_{nullptr}; //ground truth velocity in the body frame - rclcpp::Publisher::SharedPtr pub_gt_acc_{nullptr}; //ground truth acceleration in the body frame - rclcpp::Publisher::SharedPtr pub_cmd_mode{nullptr}; //for publishing command mode - rclcpp::Publisher::SharedPtr pub_state{nullptr}; //for publishing current STATE (Landed, Flying, Takingoff, Landing) - - - // PID Controller - Controllers controllers_; - - // State of the drone - ignition::math::v6::Pose3 pose; - ignition::math::v6::Vector3 euler; - ignition::math::v6::Vector3 velocity, acceleration, angular_velocity, position; - - - rclcpp::SubscriptionOptions create_subscription_options(std::string topic_name, uint32_t queue_size) - { - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; - sub_opt.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - sub_opt.topic_stats_options.publish_period = std::chrono::milliseconds(100); - sub_opt.topic_stats_options.publish_topic = std::string(topic_name + "/statistics"); - return sub_opt; - } - - // Callbacks - /** - * @brief Callback function for the drone command topic. - * This function is called whenever a new message is received on the drone command topic. It updates - * the cmd_val member variable with the new command message. It also generates motion noise for the - * drone's angular and linear velocities by adding drift and small noise values to the command message. - * The amount of noise added is determined by the motion_drift_noise_time_ and motion_small_noise_ - * member variables of the DroneSimpleController class. - * The function uses the world->SimTime() function to get the current simulator time and calculate the - * time difference between the current and last simulation time. The time difference is used to update - * the drift noise values if the time_counter_for_drift_noise is greater than motion_drift_noise_time_. - * The updated command message is then used to update the cmd_val member variable. - * - * @param cmd Pointer to the command message containing linear and angular velocities. - */ - void CmdCallback(const geometry_msgs::msg::Twist::SharedPtr cmd) - { - cmd_vel = *cmd; - - static gazebo::common::Time last_sim_time = world->SimTime(); - static double time_counter_for_drift_noise = 0; - static double drift_noise[4] = {0.0, 0.0, 0.0, 0.0}; - // Get simulator time - gazebo::common::Time cur_sim_time = world->SimTime(); - double dt = (cur_sim_time - last_sim_time).Double(); - // save last time stamp - last_sim_time = cur_sim_time; - - // generate noise - if(time_counter_for_drift_noise > motion_drift_noise_time_) - { - drift_noise[0] = 2*motion_drift_noise_*(drand48()-0.5); - drift_noise[1] = 2*motion_drift_noise_*(drand48()-0.5); - drift_noise[2] = 2*motion_drift_noise_*(drand48()-0.5); - drift_noise[3] = 2*motion_drift_noise_*(drand48()-0.5); - time_counter_for_drift_noise = 0.0; - } - time_counter_for_drift_noise += dt; - - cmd_vel.angular.x += drift_noise[0] + 2*motion_small_noise_*(drand48()-0.5); - cmd_vel.angular.y += drift_noise[1] + 2*motion_small_noise_*(drand48()-0.5); - cmd_vel.angular.z += drift_noise[3] + 2*motion_small_noise_*(drand48()-0.5); - cmd_vel.linear.z += drift_noise[2] + 2*motion_small_noise_*(drand48()-0.5); - } - - /** - * @brief Callback function for position control command. - * This function is called when a new position control command is received. - * It sets the m_posCtrl flag to the value of the command data. - * @param cmd The position control command message. - */ - void PosCtrlCallback(const std_msgs::msg::Bool::SharedPtr cmd) - { - m_posCtrl = cmd->data; - } - - /** - * @brief Callback function to handle IMU sensor data. - * @param imu Shared pointer to IMU sensor data. - * The function reads the quaternion data from the IMU sensor and updates the orientation and angular velocity of the drone. - */ - void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu) - { - //directly read the quternion from the IMU data - pose.Rot().Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z); - euler = pose.Rot().Euler(); - angular_velocity = pose.Rot().RotateVector(ignition::math::v6::Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z)); - } - - /** - * @brief Callback function to initiate taking off of the drone. - * @param msg Empty message. - */ - void TakeoffCallback(const std_msgs::msg::Empty::SharedPtr msg) - { - if(navi_state == LANDED_MODEL) - { - navi_state = TAKINGOFF_MODEL; - m_timeAfterCmd = 0; - RCLCPP_INFO(ros_node_->get_logger(), "Quadrotor takes off!!"); - } - } - - /** - * @brief Callback function to initiate landing of the drone. - * @param msg Empty message. - */ - void LandCallback(const std_msgs::msg::Empty::SharedPtr msg) - { - if(navi_state == FLYING_MODEL||navi_state == TAKINGOFF_MODEL) - { - navi_state = LANDING_MODEL; - m_timeAfterCmd = 0; - RCLCPP_INFO(ros_node_->get_logger(), "Quadrotor lands!!"); - } - } - - /** - * @brief Callback function for reset command - * This function resets the controller and the drone's state. - * @param msg Empty message - */ - void ResetCallback(const std_msgs::msg::Empty::SharedPtr msg) - { - RCLCPP_INFO(ros_node_->get_logger(), "Reset quadrotor!!"); - Reset(); - } - - /** - * @brief Callback function for receiving a message to switch between velocity and position control modes - * @param msg Shared pointer to the message containing the boolean value for switching the mode - * The function switches between velocity and position control modes based on the boolean value in the message. - * If the boolean value is true, the control mode is switched to velocity control and if it's false, the control - * mode is switched to position control. It also resets the integral term of the controllers for the new mode. - */ - void SwitchModeCallback(const std_msgs::msg::Bool::SharedPtr msg) - { - m_velMode = msg->data; - - std_msgs::msg::String mode; - if(m_velMode) - mode.data = "velocity"; - else - mode.data = "position"; - pub_cmd_mode->publish(mode); - } - - /** - * @brief Updates the current state of the drone. - * This method is responsible for updating the current state of the drone based on the navigation state. - * If the drone is taking off, it checks if the time after the command is greater than 0.5 seconds. - * If it is, it sets the navigation state to flying. If the drone is landing, it checks if the time after the command is greater than 1 second. - * If it is, it sets the navigation state to landed. If the drone is neither taking off nor landing, it resets the time after the command to zero. - * - * @param dt The time elapsed since the last update, in seconds. - */ - void UpdateState(double dt){ - if(navi_state == TAKINGOFF_MODEL){ - m_timeAfterCmd += dt; - if (m_timeAfterCmd > 0.5){ - navi_state = FLYING_MODEL; - std::cout << "Entering flying model!" << std::endl; - } - }else if(navi_state == LANDING_MODEL){ - m_timeAfterCmd += dt; - if(m_timeAfterCmd > 1.0){ - navi_state = LANDED_MODEL; - std::cout << "Landed!" <publish(state_msg); - } - - - /** - * @brief Update the dynamics of the drone. - * This method updates the dynamics of the drone based on its current state and the current - * commands being received. It computes the force and torque to be applied to the drone, and - * updates its position, velocity, and orientation accordingly. It also publishes the ground - * truth pose, velocity, and acceleration of the drone to ROS topics. - * - * @param dt The time step to use for the update. - */ - void UpdateDynamics(double dt){ - ignition::math::v6::Vector3 force, torque; - - // Get Pose/Orientation from Gazebo - pose = link->WorldPose(); - angular_velocity = link->WorldAngularVel(); - euler = pose.Rot().Euler(); - acceleration = (link->WorldLinearVel() - velocity) / dt; - velocity = link->WorldLinearVel(); - - //publish the ground truth pose of the drone to the ROS topic - geometry_msgs::msg::Pose gt_pose; - gt_pose.position.x = pose.Pos().X(); - gt_pose.position.y = pose.Pos().Y(); - gt_pose.position.z = pose.Pos().Z(); - - gt_pose.orientation.w = pose.Rot().W(); - gt_pose.orientation.x = pose.Rot().X(); - gt_pose.orientation.y = pose.Rot().Y(); - gt_pose.orientation.z = pose.Rot().Z(); - pub_gt_pose_->publish(gt_pose); - - //convert the acceleration and velocity into the body frame - ignition::math::v6::Vector3 body_vel = pose.Rot().RotateVector(velocity); - ignition::math::v6::Vector3 body_acc = pose.Rot().RotateVector(acceleration); - - //publish the velocity - geometry_msgs::msg::Twist tw; - tw.linear.x = body_vel.X(); - tw.linear.y = body_vel.Y(); - tw.linear.z = body_vel.Z(); - pub_gt_vec_->publish(tw); - - //publish the acceleration - tw.linear.x = body_acc.X(); - tw.linear.y = body_acc.Y(); - tw.linear.z = body_acc.Z(); - pub_gt_acc_->publish(tw); - - - ignition::math::v6::Vector3 poschange = pose.Pos() - position; - position = pose.Pos(); - - // Get gravity - ignition::math::v6::Vector3 gravity_body = pose.Rot().RotateVector(world->Gravity()); - double gravity = gravity_body.Length(); - double load_factor = gravity * gravity / world->Gravity().Dot(gravity_body); // Get gravity - - // Rotate vectors to coordinate frames relevant for control - ignition::math::v6::Quaternion heading_quaternion(cos(euler[2]/2), 0.0, 0.0, sin(euler[2]/2)); - ignition::math::v6::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity); - ignition::math::v6::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration); - ignition::math::v6::Vector3 angular_velocity_body = pose.Rot().RotateVectorReverse(angular_velocity); - - // update controllers - force.Set(0.0, 0.0, 0.0); - torque.Set(0.0, 0.0, 0.0); - - if( m_posCtrl){ - //position control - if(navi_state == FLYING_MODEL){ - double vx = controllers_.pos_x.update(cmd_vel.linear.x, position[0], poschange[0], dt); - double vy = controllers_.pos_y.update(cmd_vel.linear.y, position[1], poschange[1], dt); - double vz = controllers_.pos_z.update(cmd_vel.linear.z, position[2], poschange[2], dt); - - ignition::math::v6::Vector3 vb = heading_quaternion.RotateVectorReverse(ignition::math::v6::Vector3(vx,vy,vz)); - - double pitch_command = controllers_.velocity_x.update(vb[0], velocity_xy[0], acceleration_xy[0], dt) / gravity; - double roll_command = -controllers_.velocity_y.update(vb[1], velocity_xy[1], acceleration_xy[1], dt) / gravity; - torque[0] = inertia[0] * controllers_.roll.update(roll_command, euler[0], angular_velocity_body[0], dt); - torque[1] = inertia[1] * controllers_.pitch.update(pitch_command, euler[1], angular_velocity_body[1], dt); - force[2] = mass * (controllers_.velocity_z.update(vz, velocity[2], acceleration[2], dt) + load_factor * gravity); - } - }else{ - //normal control - if( navi_state == FLYING_MODEL ) - { - //hovering - double pitch_command = controllers_.velocity_x.update(cmd_vel.linear.x, velocity_xy[0], acceleration_xy[0], dt) / gravity; - double roll_command = -controllers_.velocity_y.update(cmd_vel.linear.y, velocity_xy[1], acceleration_xy[1], dt) / gravity; - torque[0] = inertia[0] * controllers_.roll.update(roll_command, euler[0], angular_velocity_body[0], dt); - torque[1] = inertia[1] * controllers_.pitch.update(pitch_command, euler[1], angular_velocity_body[1], dt); - //TODO: add hover by distnace of sonar - }else{ - //control by velocity - if( m_velMode){ - double pitch_command = controllers_.velocity_x.update(cmd_vel.angular.x, velocity_xy[0], acceleration_xy[0], dt) / gravity; - double roll_command = -controllers_.velocity_y.update(cmd_vel.angular.y, velocity_xy[1], acceleration_xy[1], dt) / gravity; - torque[0] = inertia[0] * controllers_.roll.update(roll_command, euler[0], angular_velocity_body[0], dt); - torque[1] = inertia[1] * controllers_.pitch.update(pitch_command, euler[1], angular_velocity_body[1], dt); - }else{ - //control by tilting - torque[0] = inertia[0] * controllers_.roll.update(cmd_vel.angular.x, euler[0], angular_velocity_body[0], dt); - torque[1] = inertia[1] * controllers_.pitch.update(cmd_vel.angular.y, euler[1], angular_velocity_body[1], dt); - } - } - torque[2] = inertia[2] * controllers_.yaw.update(cmd_vel.angular.z, angular_velocity[2], 0, dt); - force[2] = mass * (controllers_.velocity_z.update(cmd_vel.linear.z, velocity[2], acceleration[2], dt) + load_factor * gravity); - } - - if (max_force_ > 0.0 && force[2] > max_force_) force[2] = max_force_; - if (force[2] < 0.0) force[2] = 0.0; - - // process robot state information - if(navi_state == LANDED_MODEL) - { - - } - else if(navi_state == FLYING_MODEL) - { - link->AddRelativeForce(force); - link->AddRelativeTorque(torque); - } - else if(navi_state == TAKINGOFF_MODEL) - { - link->AddRelativeForce(force*1.5); - link->AddRelativeTorque(torque*1.5); - } - else if(navi_state == LANDING_MODEL) - { - link->AddRelativeForce(force*0.8); - link->AddRelativeTorque(torque*0.8); - } - } -}; // class DroneSimpleControllerPrivate +namespace gazebo_plugins +{ DroneSimpleController::DroneSimpleController() : impl_(std::make_unique()) @@ -581,106 +41,98 @@ void DroneSimpleController::Load(gazebo::physics::ModelPtr _model, sdf::ElementP { // Initialize ROS node impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); // TODO: Check if node_name should should be passed to Get(): https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ros2/gazebo_ros/src/node.cpp#L41C60-L41C69 + impl_->tf_broadcaster_ = std::make_unique(impl_->ros_node_); + impl_->model = _model; - if(!rclcpp::ok()){ - RCLCPP_FATAL(impl_->ros_node_->get_logger(), "A ROS node for Gazebo has not been initialized, unable to load plugin. Load the Gazebo system plugin 'libgazebo_ros_init.so' in the gazebo_ros package"); + if (!rclcpp::ok()) { + RCLCPP_FATAL( + impl_->ros_node_->get_logger(), + "A ROS node for Gazebo has not been initialized, unable to load plugin. Load the Gazebo system plugin 'libgazebo_ros_init.so' in the gazebo_ros package"); exit(1); } - + world = _model->GetWorld(); impl_->world = _model->GetWorld(); - RCLCPP_INFO(impl_->ros_node_->get_logger(), "The drone plugin is loading!"); - - if (!_sdf->HasElement("bodyName")) - { - RCLCPP_INFO(impl_->ros_node_->get_logger(), "The drone plugin is loading1!"); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "The drone plugin is loading!"); + + if (!_sdf->HasElement("bodyName")) { impl_->link = _model->GetLink(); link_name_ = impl_->link->GetName(); - } - else { - RCLCPP_INFO(impl_->ros_node_->get_logger(), "The drone plugin is loading2!"); + } else { link_name_ = _sdf->GetElement("bodyName")->Get(); - auto entity = world->EntityByName(link_name_); - // if (entity) { - impl_->link = boost::dynamic_pointer_cast(entity); - RCLCPP_INFO(impl_->ros_node_->get_logger(), "The drone plugin is loading3!"); - // } - // else { - // RCLCPP_FATAL(impl_->ros_node_->get_logger(), "bodyName: %s does not exist\n", link_name_.c_str()); - // return; - // } + // auto entity = world->EntityByName(link_name_); + // impl_->link = boost::dynamic_pointer_cast(entity); + impl_->link = + boost::dynamic_pointer_cast(world->EntityByName(link_name_)); } - RCLCPP_INFO(impl_->ros_node_->get_logger(), "1"); - if (!impl_->link) - { - RCLCPP_FATAL(impl_->ros_node_->get_logger(), "gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str()); + if (!impl_->link) { + RCLCPP_FATAL( + impl_->ros_node_->get_logger(), "gazebo_ros_baro plugin error: bodyName: %s does not exist\n", + link_name_.c_str()); return; } - RCLCPP_INFO(impl_->ros_node_->get_logger(), "1"); - if (!_sdf->HasElement("maxForce")) + if (!_sdf->HasElement("pub_odom")) { + impl_->pub_odom = false; + } else { + impl_->pub_odom = _sdf->GetElement("pub_odom")->Get(); + if(!_sdf->HasElement("odom_hz")) { + impl_->odom_hz = 30; + } else { + impl_->odom_hz = _sdf->GetElement("odom_hz")->Get(); + } + } + RCLCPP_INFO(impl_->ros_node_->get_logger(), "pub_odom: %d", impl_->pub_odom); + + if (!_sdf->HasElement("maxForce")) { impl_->max_force_ = -1; - else + } else { impl_->max_force_ = _sdf->GetElement("maxForce")->Get(); + } - if (!_sdf->HasElement("motionSmallNoise")) + if (!_sdf->HasElement("motionSmallNoise")) { impl_->motion_small_noise_ = 0; - else - impl_-> motion_small_noise_ = _sdf->GetElement("motionSmallNoise")->Get(); + } else { + impl_->motion_small_noise_ = _sdf->GetElement("motionSmallNoise")->Get(); + } - if (!_sdf->HasElement("motionDriftNoise")) - impl_->motion_drift_noise_ = 0; - else + if (!_sdf->HasElement("motionDriftNoise")) { + impl_->motion_drift_noise_ = 0; + } else { impl_->motion_drift_noise_ = _sdf->GetElement("motionDriftNoise")->Get(); + } - if (!_sdf->HasElement("motionDriftNoiseTime")) + if (!_sdf->HasElement("motionDriftNoiseTime")) { impl_->motion_drift_noise_time_ = 1.0; - else + } else { impl_->motion_drift_noise_time_ = _sdf->GetElement("motionDriftNoiseTime")->Get(); + } - - RCLCPP_INFO_STREAM(impl_->ros_node_->get_logger(), "Using following parameters: \n" << - "\t\tlink_name: "<< link_name_.c_str() << ",\n" << - "\t\tmax_force: "<< impl_->max_force_ << ",\n" << - "\t\tmotion_small_noise: "<< impl_->motion_small_noise_ << ",\n" << - "\t\tmotion_drift_noise: "<< impl_-> motion_drift_noise_ << ",\n" << - "\t\tmotion_drift_noise_time: "<< impl_->motion_drift_noise_time_ - ); + RCLCPP_INFO_STREAM( + impl_->ros_node_->get_logger(), "Using following parameters: \n" << + "\t\tlink_name: " << link_name_.c_str() << ",\n" << + "\t\tmax_force: " << impl_->max_force_ << ",\n" << + "\t\tmotion_small_noise: " << impl_->motion_small_noise_ << ",\n" << + "\t\tmotion_drift_noise: " << impl_->motion_drift_noise_ << ",\n" << + "\t\tmotion_drift_noise_time: " << impl_->motion_drift_noise_time_ + ); // get inertia and mass of quadrotor body impl_->inertia = impl_->link->GetInertial()->PrincipalMoments(); impl_->mass = impl_->link->GetInertial()->Mass(); - // Subscriber topics - //TODO: read from sdf / xacro - const std::string cmd_normal_topic_ = "cmd_vel"; - const std::string imu_topic_ = "imu"; - const std::string takeoff_topic_ = "takeoff"; - const std::string land_topic_ = "land"; - const std::string reset_topic_ = "reset"; - const std::string posctrl_topic_ = "posctrl"; - const std::string switch_mode_topic_ = "dronevel_mode"; - - // Publisher topics - //TODO: read from sdf / xacro - const std::string gt_topic_ = "gt_pose"; - const std::string gt_vel_topic_ = "gt_vel"; - const std::string gt_acc_topic_ = "gt_acc"; - const std::string cmd_mode_topic_ = "cmd_mode"; - const std::string state_topic_ = "state"; - - impl_->InitSubscribers(cmd_normal_topic_, posctrl_topic_, imu_topic_, takeoff_topic_, land_topic_, reset_topic_, switch_mode_topic_); - impl_->InitPublishers(gt_topic_, gt_vel_topic_, gt_acc_topic_, cmd_mode_topic_, state_topic_); + impl_->InitSubscribers(); + impl_->InitPublishers(); impl_->LoadControllerSettings(_model, _sdf); - + Reset(); updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&DroneSimpleController::Update, this)); + std::bind(&DroneSimpleController::Update, this, std::placeholders::_1)); RCLCPP_INFO(impl_->ros_node_->get_logger(), "The drone plugin finished loading!"); } @@ -688,18 +140,17 @@ void DroneSimpleController::Load(gazebo::physics::ModelPtr _model, sdf::ElementP /** * @brief Update method called by the Gazebo simulator every simulation iteration. */ -void DroneSimpleController::Update() -{ - // Get simulator time - const gazebo::common::Time sim_time = world->SimTime(); - const double dt = (sim_time - last_time).Double(); - if (dt == 0.0) return; - +void DroneSimpleController::Update(const gazebo::common::UpdateInfo & _info) +{ + impl_->current_time = _info.simTime; + const double dt = (impl_->current_time - impl_->last_time).Double(); + if (dt == 0.0) {return;} + impl_->UpdateState(dt); impl_->UpdateDynamics(dt); - + // save last time stamp - last_time = sim_time; + impl_->last_time = impl_->current_time; } void DroneSimpleController::Reset() @@ -710,4 +161,4 @@ void DroneSimpleController::Reset() // Register this plugin with the simulator GZ_REGISTER_MODEL_PLUGIN(DroneSimpleController) -} // namespace gazebo_plugins \ No newline at end of file +} // namespace gazebo_plugins diff --git a/sjtu_drone_description/src/plugin_drone_private.cpp b/sjtu_drone_description/src/plugin_drone_private.cpp new file mode 100644 index 0000000..f19aa92 --- /dev/null +++ b/sjtu_drone_description/src/plugin_drone_private.cpp @@ -0,0 +1,596 @@ +// Copyright 2024 Georg Novotny +// +// Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.gnu.org/licenses/gpl-3.0.en.html +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gazebo_ros/conversions/builtin_interfaces.hpp" +#include + +#include "sjtu_drone_description/plugin_drone_private.h" + +namespace gazebo_plugins +{ + +DroneSimpleControllerPrivate::DroneSimpleControllerPrivate() +: m_timeAfterCmd(0.0) + , navi_state(LANDED_MODEL) + , m_posCtrl(false) + , m_velMode(false) + , odom_seq(0) + , odom_hz(30) + , last_odom_publish_time_(0.0) +{ +} + +DroneSimpleControllerPrivate::~DroneSimpleControllerPrivate() {} + +void DroneSimpleControllerPrivate::Reset() +{ + // Reset the values of the controllers + controllers_.roll.reset(); + controllers_.pitch.reset(); + controllers_.yaw.reset(); + controllers_.velocity_x.reset(); + controllers_.velocity_y.reset(); + controllers_.velocity_z.reset(); + + // Set the force and torque acting on the drone to zero + link->SetForce(ignition::math::Vector3(0.0, 0.0, 0.0)); + link->SetTorque(ignition::math::v6::Vector3(0.0, 0.0, 0.0)); + + // Reset the state of the drone + pose.Reset(); + velocity.Set(); + angular_velocity.Set(); + acceleration.Set(); + euler.Set(); +} + +// ROS Node configuration +void DroneSimpleControllerPrivate::InitSubscribers( + std::string cmd_normal_topic_, + std::string posctrl_topic_, + std::string imu_topic_, + std::string takeoff_topic_, + std::string land_topic_, + std::string reset_topic_, + std::string switch_mode_topic_) +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(); + + if (!cmd_normal_topic_.empty()) { + cmd_subscriber_ = ros_node_->create_subscription( + cmd_normal_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::CmdCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No cmd_topic defined!"); + } + + if (!posctrl_topic_.empty()) { + posctrl_subscriber_ = ros_node_->create_subscription( + posctrl_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::PosCtrlCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No position control defined!"); + } + + // subscribe imu + if (!imu_topic_.empty()) { + imu_subscriber_ = ros_node_->create_subscription( + imu_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::ImuCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No imu topic defined!"); + } + + // subscribe command: take off command + if (!takeoff_topic_.empty()) { + takeoff_subscriber_ = ros_node_->create_subscription( + takeoff_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::TakeoffCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No takeoff topic defined!"); + } + + // subscribe command: land command + if (!land_topic_.empty()) { + land_subscriber_ = ros_node_->create_subscription( + land_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::LandCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No land topic defined!"); + } + + // subscribe command: reset command + if (!reset_topic_.empty()) { + reset_subscriber_ = ros_node_->create_subscription( + reset_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::ResetCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No reset topic defined!"); + } + + // Subscribe command: switch mode command + if (!switch_mode_topic_.empty()) { + switch_mode_subscriber_ = ros_node_->create_subscription( + switch_mode_topic_, qos, + std::bind(&DroneSimpleControllerPrivate::SwitchModeCallback, this, std::placeholders::_1)); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No switch mode topic defined!"); + } +} + +void DroneSimpleControllerPrivate::InitPublishers( + std::string gt_topic_, std::string gt_vel_topic_, + std::string gt_acc_topic_, + std::string cmd_mode_topic_, + std::string state_topic_, + std::string odom_topic_) +{ + if (!gt_topic_.empty()) { + pub_gt_pose_ = ros_node_->create_publisher(gt_topic_, 1024); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No ground truth topic defined!"); + } + + if (!gt_vel_topic_.empty()) { + pub_gt_vec_ = ros_node_->create_publisher("gt_vel", 1024); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No ground truth velocity topic defined!"); + } + + if (!gt_acc_topic_.empty()) { + pub_gt_acc_ = ros_node_->create_publisher("gt_acc", 1024); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No ground truth acceleration topic defined!"); + } + + if (!cmd_mode_topic_.empty()) { + pub_cmd_mode = ros_node_->create_publisher(cmd_mode_topic_, 1024); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No command mode topic defined!"); + } + + if (!state_topic_.empty()) { + pub_state = ros_node_->create_publisher(state_topic_, 1024); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No state topic defined!"); + } + + if (!odom_topic_.empty()) { + pub_odom_ = ros_node_->create_publisher(odom_topic_, 1024); + } else { + RCLCPP_ERROR(ros_node_->get_logger(), "No odom topic defined!"); + } +} + + + +// Controller configuration +/** +* @brief Initiliaze the PID params +* +* @param _model shared pointer to the model object +* @param _sdf shared pointer to the sdf object +*/ +void DroneSimpleControllerPrivate::LoadControllerSettings( + gazebo::physics::ModelPtr _model, + sdf::ElementPtr _sdf) +{ + controllers_.roll.Load(_sdf, "rollpitch"); + controllers_.pitch.Load(_sdf, "rollpitch"); + controllers_.yaw.Load(_sdf, "yaw"); + controllers_.velocity_x.Load(_sdf, "velocityXY"); + controllers_.velocity_y.Load(_sdf, "velocityXY"); + controllers_.velocity_z.Load(_sdf, "velocityZ"); + + controllers_.pos_x.Load(_sdf, "positionXY"); + controllers_.pos_y.Load(_sdf, "positionXY"); + controllers_.pos_z.Load(_sdf, "positionZ"); + + RCLCPP_INFO_STREAM( + ros_node_->get_logger(), "Using the PID parameters: \n" << + "\tRoll Pitch:\n" << "\t\tkP: " << controllers_.roll.gain_p << ", kI: " << controllers_.roll.gain_i << ",kD: " << controllers_.roll.gain_d << ", Limit: " << controllers_.roll.limit << ", Time Constant: " << controllers_.roll.time_constant << "\n" << + "\tYaw:\n" << "\t\tkP: " << controllers_.yaw.gain_p << ", kI: " << controllers_.yaw.gain_i << ",kD: " << controllers_.yaw.gain_d << ", Limit: " << controllers_.yaw.limit << ", Time Constant: " << controllers_.yaw.time_constant << "\n" << + "\tVelocity X:\n" << "\t\tkP: " << controllers_.velocity_x.gain_p << ", kI: " << controllers_.velocity_x.gain_i << ",kD: " << controllers_.velocity_x.gain_d << ", Limit: " << controllers_.velocity_x.limit << ", Time Constant: " << controllers_.velocity_x.time_constant << "\n" << + "\tVelocity Y:\n" << "\t\tkP: " << controllers_.velocity_y.gain_p << ", kI: " << controllers_.velocity_y.gain_i << ",kD: " << controllers_.velocity_y.gain_d << ", Limit: " << controllers_.velocity_y.limit << ", Time Constant: " << controllers_.velocity_y.time_constant << "\n" << + "\tVelocity Z:\n" << "\t\tkP: " << controllers_.velocity_z.gain_p << ", kI: " << controllers_.velocity_z.gain_i << ",kD: " << controllers_.velocity_z.gain_d << ", Limit: " << controllers_.velocity_z.limit << ", Time Constant: " << controllers_.velocity_z.time_constant << "\n" << + "\tPosition XY:\n" << "\t\tkP: " << controllers_.pos_x.gain_p << ", kI: " << controllers_.pos_x.gain_i << ",kD: " << controllers_.pos_x.gain_d << ", Limit: " << controllers_.pos_x.limit << ", Time Constant: " << controllers_.pos_x.time_constant << "\n" << + "\tPosition Z:\n" << "\t\tkP: " << controllers_.pos_z.gain_p << ", kI: " << controllers_.pos_z.gain_i << ",kD: " << controllers_.pos_z.gain_d << ", Limit: " << controllers_.pos_z.limit << ", Time Constant: " << controllers_.pos_z.time_constant + ); +} + +// Callbacks +/** +* @brief Callback function for the drone command topic. +* This function is called whenever a new message is received on the drone command topic. It updates +* the cmd_val member variable with the new command message. It also generates motion noise for the +* drone's angular and linear velocities by adding drift and small noise values to the command message. +* The amount of noise added is determined by the motion_drift_noise_time_ and motion_small_noise_ +* member variables of the DroneSimpleController class. +* The function uses the world->SimTime() function to get the current simulator time and calculate the +* time difference between the current and last simulation time. The time difference is used to update +* the drift noise values if the time_counter_for_drift_noise is greater than motion_drift_noise_time_. +* The updated command message is then used to update the cmd_val member variable. +* +* @param cmd Pointer to the command message containing linear and angular velocities. +*/ +void DroneSimpleControllerPrivate::CmdCallback(const geometry_msgs::msg::Twist::SharedPtr cmd) +{ + cmd_vel = *cmd; + + static gazebo::common::Time last_sim_time = world->SimTime(); + static double time_counter_for_drift_noise = 0; + static double drift_noise[4] = {0.0, 0.0, 0.0, 0.0}; + // Get simulator time + gazebo::common::Time cur_sim_time = world->SimTime(); + double dt = (cur_sim_time - last_sim_time).Double(); + // save last time stamp + last_sim_time = cur_sim_time; + + // generate noise + if (time_counter_for_drift_noise > motion_drift_noise_time_) { + drift_noise[0] = 2 * motion_drift_noise_ * (drand48() - 0.5); + drift_noise[1] = 2 * motion_drift_noise_ * (drand48() - 0.5); + drift_noise[2] = 2 * motion_drift_noise_ * (drand48() - 0.5); + drift_noise[3] = 2 * motion_drift_noise_ * (drand48() - 0.5); + time_counter_for_drift_noise = 0.0; + } + time_counter_for_drift_noise += dt; + + cmd_vel.angular.x += drift_noise[0] + 2 * motion_small_noise_ * (drand48() - 0.5); + cmd_vel.angular.y += drift_noise[1] + 2 * motion_small_noise_ * (drand48() - 0.5); + cmd_vel.angular.z += drift_noise[3] + 2 * motion_small_noise_ * (drand48() - 0.5); + cmd_vel.linear.z += drift_noise[2] + 2 * motion_small_noise_ * (drand48() - 0.5); +} + +/** +* @brief Callback function for position control command. +* This function is called when a new position control command is received. +* It sets the m_posCtrl flag to the value of the command data. +* @param cmd The position control command message. +*/ +void DroneSimpleControllerPrivate::PosCtrlCallback(const std_msgs::msg::Bool::SharedPtr cmd) +{ + m_posCtrl = cmd->data; +} + +/** +* @brief Callback function to handle IMU sensor data. +* @param imu Shared pointer to IMU sensor data. +* The function reads the quaternion data from the IMU sensor and updates the orientation and angular velocity of the drone. +*/ +void DroneSimpleControllerPrivate::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu) +{ + //directly read the quternion from the IMU data + pose.Rot().Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z); + euler = pose.Rot().Euler(); + angular_velocity = + pose.Rot().RotateVector( + ignition::math::v6::Vector3( + imu->angular_velocity.x, + imu->angular_velocity.y, imu->angular_velocity.z)); +} + +/** +* @brief Callback function to initiate taking off of the drone. +* @param msg Empty message. +*/ +void DroneSimpleControllerPrivate::TakeoffCallback(const std_msgs::msg::Empty::SharedPtr msg) +{ + if (navi_state == LANDED_MODEL) { + navi_state = TAKINGOFF_MODEL; + m_timeAfterCmd = 0; + RCLCPP_INFO(ros_node_->get_logger(), "Quadrotor takes off!!"); + } +} + +/** +* @brief Callback function to initiate landing of the drone. +* @param msg Empty message. +*/ +void DroneSimpleControllerPrivate::LandCallback(const std_msgs::msg::Empty::SharedPtr msg) +{ + if (navi_state == FLYING_MODEL || navi_state == TAKINGOFF_MODEL) { + navi_state = LANDING_MODEL; + m_timeAfterCmd = 0; + RCLCPP_INFO(ros_node_->get_logger(), "Quadrotor lands!!"); + } +} + +/** +* @brief Callback function for reset command +* This function resets the controller and the drone's state. +* @param msg Empty message +*/ +void DroneSimpleControllerPrivate::ResetCallback(const std_msgs::msg::Empty::SharedPtr msg) +{ + RCLCPP_INFO(ros_node_->get_logger(), "Reset quadrotor!!"); + Reset(); +} + +/** +* @brief Callback function for receiving a message to switch between velocity and position control modes +* @param msg Shared pointer to the message containing the boolean value for switching the mode +* The function switches between velocity and position control modes based on the boolean value in the message. +* If the boolean value is true, the control mode is switched to velocity control and if it's false, the control +* mode is switched to position control. It also resets the integral term of the controllers for the new mode. +*/ +void DroneSimpleControllerPrivate::SwitchModeCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + m_velMode = msg->data; + + std_msgs::msg::String mode; + if (m_velMode) { + mode.data = "velocity"; + } else { + mode.data = "position"; + } + pub_cmd_mode->publish(mode); +} + + +void DroneSimpleControllerPrivate::PublishOdom( + const ignition::math::v6::Pose3 & pose, + const ignition::math::v6::Vector3 & velocity, + const ignition::math::v6::Vector3 & acceleration) +{ + // Prepare the Odometry message + nav_msgs::msg::Odometry odom; + odom.header.stamp = gazebo_ros::Convert(current_time); + std::string ns = ros_node_->get_namespace(); + odom.header.frame_id = ns + "/odom"; + odom.child_frame_id = ns + "/base_footprint"; + + // Set position and orientation from the drone's current state in UpdateDynamics + odom.pose.pose.position = gazebo_ros::Convert(pose.Pos()); + odom.pose.pose.orientation = gazebo_ros::Convert(pose.Rot()); + + // Set velocity information from UpdateDynamics + auto linear = model->WorldLinearVel(); + auto angular = model->WorldAngularVel(); + + // Convert velocities to child frame (aka base_footprint) + auto pose_rot = pose.Rot(); + auto pose_rot_inv = pose_rot.Inverse(); + auto linear_child = pose_rot_inv.RotateVector(linear); + auto angular_child = pose_rot_inv.RotateVector(angular); + odom.twist.twist.linear = gazebo_ros::Convert(linear_child); + odom.twist.twist.angular = gazebo_ros::Convert(angular_child); + // Publish the odometry message + pub_odom_->publish(odom); + + // Publish the TF transformation + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = odom.header.stamp; + transformStamped.header.frame_id = ns + "/odom"; + transformStamped.child_frame_id = ns + "/base_footprint"; + transformStamped.transform.translation.x = odom.pose.pose.position.x; + transformStamped.transform.translation.y = odom.pose.pose.position.y; + transformStamped.transform.translation.z = odom.pose.pose.position.z; + transformStamped.transform.rotation = odom.pose.pose.orientation; + + tf_broadcaster_->sendTransform(transformStamped); +} + + +/** +* @brief Updates the current state of the drone. +* This method is responsible for updating the current state of the drone based on the navigation state. +* If the drone is taking off, it checks if the time after the command is greater than 0.5 seconds. +* If it is, it sets the navigation state to flying. If the drone is landing, it checks if the time after the command is greater than 1 second. +* If it is, it sets the navigation state to landed. If the drone is neither taking off nor landing, it resets the time after the command to zero. +* +* @param dt The time elapsed since the last update, in seconds. +*/ +void DroneSimpleControllerPrivate::UpdateState(double dt) +{ + if (navi_state == TAKINGOFF_MODEL) { + m_timeAfterCmd += dt; + if (m_timeAfterCmd > 0.5) { + navi_state = FLYING_MODEL; + std::cout << "Entering flying model!" << std::endl; + } + } else if (navi_state == LANDING_MODEL) { + m_timeAfterCmd += dt; + if (m_timeAfterCmd > 1.0) { + navi_state = LANDED_MODEL; + std::cout << "Landed!" << std::endl; + } + } else { + m_timeAfterCmd = 0; + } + + // publish current state using pub_state + std_msgs::msg::Int8 state_msg; + state_msg.data = navi_state; + pub_state->publish(state_msg); +} + + +/** +* @brief Update the dynamics of the drone. +* This method updates the dynamics of the drone based on its current state and the current +* commands being received. It computes the force and torque to be applied to the drone, and +* updates its position, velocity, and orientation accordingly. It also publishes the ground +* truth pose, velocity, and acceleration of the drone to ROS topics. +* +* @param dt The time step to use for the update. +*/ +void DroneSimpleControllerPrivate::UpdateDynamics(double dt) +{ + ignition::math::v6::Vector3 force, torque; + + // Get Pose/Orientation from Gazebo + pose = link->WorldPose(); + angular_velocity = link->WorldAngularVel(); + euler = pose.Rot().Euler(); + acceleration = (link->WorldLinearVel() - velocity) / dt; + velocity = link->WorldLinearVel(); + + //publish the ground truth pose of the drone to the ROS topic + geometry_msgs::msg::Pose gt_pose; + gt_pose.position.x = pose.Pos().X(); + gt_pose.position.y = pose.Pos().Y(); + gt_pose.position.z = pose.Pos().Z(); + + gt_pose.orientation.w = pose.Rot().W(); + gt_pose.orientation.x = pose.Rot().X(); + gt_pose.orientation.y = pose.Rot().Y(); + gt_pose.orientation.z = pose.Rot().Z(); + pub_gt_pose_->publish(gt_pose); + + //convert the acceleration and velocity into the body frame + ignition::math::v6::Vector3 body_vel = pose.Rot().RotateVector(velocity); + ignition::math::v6::Vector3 body_acc = pose.Rot().RotateVector(acceleration); + + //publish the velocity + geometry_msgs::msg::Twist tw; + tw.linear.x = body_vel.X(); + tw.linear.y = body_vel.Y(); + tw.linear.z = body_vel.Z(); + pub_gt_vec_->publish(tw); + + //publish the acceleration + tw.linear.x = body_acc.X(); + tw.linear.y = body_acc.Y(); + tw.linear.z = body_acc.Z(); + pub_gt_acc_->publish(tw); + + + ignition::math::v6::Vector3 poschange = pose.Pos() - position; + position = pose.Pos(); + + // Get gravity + ignition::math::v6::Vector3 gravity_body = pose.Rot().RotateVector(world->Gravity()); + double gravity = gravity_body.Length(); + double load_factor = gravity * gravity / world->Gravity().Dot(gravity_body); // Get gravity + + // Rotate vectors to coordinate frames relevant for control + ignition::math::v6::Quaternion heading_quaternion(cos(euler[2] / 2), 0.0, 0.0, + sin(euler[2] / 2)); + ignition::math::v6::Vector3 velocity_xy = + heading_quaternion.RotateVectorReverse(velocity); + ignition::math::v6::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse( + acceleration); + ignition::math::v6::Vector3 angular_velocity_body = pose.Rot().RotateVectorReverse( + angular_velocity); + + // update controllers + force.Set(0.0, 0.0, 0.0); + torque.Set(0.0, 0.0, 0.0); + + if (m_posCtrl) { + //position control + if (navi_state == FLYING_MODEL) { + double vx = controllers_.pos_x.update(cmd_vel.linear.x, position[0], poschange[0], dt); + double vy = controllers_.pos_y.update(cmd_vel.linear.y, position[1], poschange[1], dt); + double vz = controllers_.pos_z.update(cmd_vel.linear.z, position[2], poschange[2], dt); + + ignition::math::v6::Vector3 vb = heading_quaternion.RotateVectorReverse( + ignition::math::v6::Vector3( + vx, vy, + vz)); + + double pitch_command = controllers_.velocity_x.update( + vb[0], velocity_xy[0], + acceleration_xy[0], dt) / gravity; + double roll_command = -controllers_.velocity_y.update( + vb[1], velocity_xy[1], + acceleration_xy[1], dt) / gravity; + torque[0] = inertia[0] * controllers_.roll.update( + roll_command, euler[0], + angular_velocity_body[0], dt); + torque[1] = inertia[1] * controllers_.pitch.update( + pitch_command, euler[1], + angular_velocity_body[1], dt); + force[2] = mass * + (controllers_.velocity_z.update( + vz, velocity[2], acceleration[2], + dt) + load_factor * gravity); + } + } else { + //normal control + if (navi_state == FLYING_MODEL) { + //hovering + double pitch_command = controllers_.velocity_x.update( + cmd_vel.linear.x, velocity_xy[0], + acceleration_xy[0], dt) / gravity; + double roll_command = -controllers_.velocity_y.update( + cmd_vel.linear.y, velocity_xy[1], + acceleration_xy[1], dt) / gravity; + torque[0] = inertia[0] * controllers_.roll.update( + roll_command, euler[0], + angular_velocity_body[0], dt); + torque[1] = inertia[1] * controllers_.pitch.update( + pitch_command, euler[1], + angular_velocity_body[1], dt); + //TODO: add hover by distnace of sonar + } else { + //control by velocity + if (m_velMode) { + double pitch_command = controllers_.velocity_x.update( + cmd_vel.angular.x, velocity_xy[0], + acceleration_xy[0], dt) / gravity; + double roll_command = -controllers_.velocity_y.update( + cmd_vel.angular.y, velocity_xy[1], + acceleration_xy[1], dt) / gravity; + torque[0] = inertia[0] * controllers_.roll.update( + roll_command, euler[0], + angular_velocity_body[0], dt); + torque[1] = inertia[1] * controllers_.pitch.update( + pitch_command, euler[1], + angular_velocity_body[1], dt); + } else { + //control by tilting + torque[0] = inertia[0] * controllers_.roll.update( + cmd_vel.angular.x, euler[0], + angular_velocity_body[0], dt); + torque[1] = inertia[1] * controllers_.pitch.update( + cmd_vel.angular.y, euler[1], + angular_velocity_body[1], dt); + } + } + torque[2] = inertia[2] * controllers_.yaw.update(cmd_vel.angular.z, angular_velocity[2], 0, dt); + force[2] = mass * + (controllers_.velocity_z.update( + cmd_vel.linear.z, velocity[2], acceleration[2], + dt) + load_factor * gravity); + } + + if (max_force_ > 0.0 && force[2] > max_force_) {force[2] = max_force_;} + if (force[2] < 0.0) {force[2] = 0.0;} + + // process robot state information + if (navi_state == LANDED_MODEL) { + + } else if (navi_state == FLYING_MODEL) { + link->AddRelativeForce(force); + link->AddRelativeTorque(torque); + } else if (navi_state == TAKINGOFF_MODEL) { + link->AddRelativeForce(force * 1.5); + link->AddRelativeTorque(torque * 1.5); + } else if (navi_state == LANDING_MODEL) { + link->AddRelativeForce(force * 0.8); + link->AddRelativeTorque(torque * 0.8); + } + + if (pub_odom) { + if (current_time.Double() - last_odom_publish_time_ >= 1.0 / odom_hz) { + PublishOdom(pose, velocity, acceleration); + last_odom_publish_time_ = current_time.Double(); + odom_seq++; + } + } +} + +} // namespace gazebo_plugins diff --git a/sjtu_drone_description/urdf/sjtu_drone.urdf.xacro b/sjtu_drone_description/urdf/sjtu_drone.urdf.xacro index 3524c2e..bf1c968 100644 --- a/sjtu_drone_description/urdf/sjtu_drone.urdf.xacro +++ b/sjtu_drone_description/urdf/sjtu_drone.urdf.xacro @@ -80,6 +80,7 @@ base_link + true ${mp['rollpitchProportionalGain']} ${mp['rollpitchDifferentialGain']} ${mp['rollpitchLimit']} @@ -121,17 +122,18 @@ gaussian 0 - 0 + 0.01 0 - 0.00 + 0.01 ${mp['namespace']} + false ${mp['namespace']}/base_link @@ -203,7 +205,7 @@ gaussian 0.0 - 0.005 + 0.01 @@ -240,7 +242,7 @@ gaussian 0.0 - 0.005 + 0.01 @@ -286,4 +288,22 @@ + + + + + 100.0 + true + + base_footprint_fixed_joint_lump__sjtu_drone_collision_collision + + + + ${mp['namespace']} + + + ${mp['namespace']}/base_link + + +