From f44f1ff9b02f435edd7f20ee5dd97f58d21551de Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 21:27:54 +0200 Subject: [PATCH] comments --- src/px4_connection/src/px4_controller.cpp | 132 +++++++++++++--------- 1 file changed, 78 insertions(+), 54 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e7a428d1..1bf94796 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -38,27 +38,27 @@ class PX4Controller : public rclcpp::Node public: PX4Controller() : Node("px4_controller") { - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); - // create a publisher on the vehicle attitude setpoint topic + // publishers for controlling the drone vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); + // publisher for publishing if the drone is armed arm_status_publisher_ = this->create_publisher("/drone/arm_status", 10); - // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + // subscriptions from the Pixhawk vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1)); vehicle_local_position_subscription_ = this->create_subscription("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); + // subscription on receiving a new control mode control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1)); - - + // services for controlling the drone set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); disarm_service_ = this->create_service("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - // create timer to send vehicle attitude setpoints every second + // create timer to send setpoints every 100 milliseconds timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); start_time_ = this->get_clock()->now().seconds(); @@ -80,44 +80,37 @@ private: rclcpp::Service::SharedPtr disarm_service_; rclcpp::Service::SharedPtr arm_service_; - // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - double start_time_; - bool has_sent_status = false; + double start_time_; // time when the node was started bool user_in_control = false; // if user has taken over control - bool armed = false; - bool has_swithed = false; - int setpoint_count = 0; - float thrust = 0.0; - bool ready_to_fly = false; - float cur_yaw = 0; - bool new_setpoint = false; // for printing new q_d when a new setpoint has been received + bool armed = false; // if the drone is armed + bool ready_to_fly = false; // if the drone is ready to fly + bool new_setpoint = false; // for printing new q_d when a new setpoint has been received - float last_setpoint[3] = {0, 0, 0}; - float last_angle = 0; // angle in radians (-PI .. PI) - float last_thrust = 0; // default 10% thrust for when the drone gets armed + float last_setpoint[3] = {0, 0, 0}; // yaw, pitch, roll + float last_angle = 0; // angle in radians (-PI .. PI) + float last_thrust = 0; // default 10% thrust for when the drone gets armed - float velocity[3] = {0, 0, 0}; - float position[3] = {0, 0, 0}; + float velocity[3] = {0, 0, 0}; // velocity setpoint [x,y,z] + float position[3] = {0, 0, 0}; // position setpoint [x,y,z] - float base_q[4] = {0, 0, 0, 0}; - int base_q_amount = 0; + float base_q[4] = {0, 0, 0, 0}; // base local position quaternion + int base_q_amount = 0; // amount of base quaternions received char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control - bool start_trajectory = false; - const float omega = 0.3; // angular speed of the POLAR trajectory - const float K = 0; // [m] gain that regulates the spiral pitch + bool start_trajectory = false; // start trajectory when drone is at start position + const float omega = 0.3; // angular speed of the POLAR trajectory + const float K = 0; // [m] gain that regulates the spiral pitch - float rho_0 = 0; - float theta_0 = 0; - const float p0_z = -0.0; - float p0_x = rho_0 * cos(theta_0); - float p0_y = rho_0 * sin(theta_0); - float des_x = p0_x, des_y = p0_y, des_z = p0_z; - float dot_des_x = 0.0, dot_des_y = 0.0; - float gamma = M_PI_4; + float rho_0 = 0; // initial rho of polar coordinate + float theta_0 = 0; // initial theta of polar coordinate + const float p0_z = -0.0; // initial height + float p0_x = rho_0 * cos(theta_0); // initial x position + float p0_y = rho_0 * sin(theta_0); // initial y position + float des_x = p0_x, des_y = p0_y, des_z = p0_z; // desired position + float dot_des_x = 0.0, dot_des_y = 0.0; // desired velocity + float gamma = M_PI_4; // desired heading direction float X; float Y; @@ -127,9 +120,15 @@ private: uint32_t discrete_time_index = 0; - // result quaternion - std::array q = {0, 0, 0, 0}; + std::array q = {0, 0, 0, 0}; // result quaternion + /** + * @brief handles reception of attitude setpoints through service + * + * @param request_header the header of the request + * @param request the request containing the new attitude setpoint + * @param response the response indicating if the setpoint was changed successfully + */ void handle_attitude_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, @@ -172,6 +171,13 @@ private: } } + /** + * @brief handles reception of trajectory setpoints through service + * + * @param request_header the header of the request + * @param request the request containing the new trajectory setpoint (velocity or position) + * @param response the response indicating if the setpoint was changed successfully + */ void handle_trajectory_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, @@ -232,7 +238,7 @@ private: auto msg = drone_services::msg::DroneArmStatus(); msg.armed = false; arm_status_publisher_->publish(msg); - + response->success = true; } else @@ -280,6 +286,10 @@ private: } } + /** + * @brief sends the latest received attitude setpoint to the drone + * + */ void send_attitude_setpoint() { @@ -306,6 +316,10 @@ private: vehicle_setpoint_publisher_->publish(msg); } + /** + * @brief sends the latest received velocity setpoint to the drone + * + */ void send_velocity_setpoint() { auto msg = px4_msgs::msg::TrajectorySetpoint(); @@ -313,20 +327,24 @@ private: msg.velocity[0] = velocity[0]; msg.velocity[1] = velocity[1]; msg.velocity[2] = -velocity[2]; + // set no position control for (int i = 0; i < 3; i++) { msg.position[i] = NAN; } - publish_trajectory_setpoint(msg); } + /** + * @brief sends the latest received position setpoint to the drone + * + */ void send_position_setpoint() { auto msg = px4_msgs::msg::TrajectorySetpoint(); RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); - RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y); + RCLCPP_INFO(this->get_logger(), "local position: %f %f", local_x, local_y); msg.position = {local_x, local_y, des_z}; msg.velocity = {dot_des_x, dot_des_y, 0.0}; msg.yaw = gamma; //-3.14; // [-PI:PI] @@ -335,6 +353,11 @@ private: trajectory_setpoint_publisher->publish(msg); } + /** + * @brief publishes a trajectory setpoint + * + * @param msg the message containing the trajectory setpoint + */ void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) { msg.yaw = last_angle; @@ -344,7 +367,7 @@ private: } /** - * @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed. + * @brief Send setpoints to PX4. * */ void send_setpoint() @@ -376,7 +399,6 @@ private: { if (current_control_mode == CONTROL_MODE_ATTITUDE) { - // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); send_attitude_setpoint(); } else @@ -387,12 +409,10 @@ private: } if (current_control_mode == CONTROL_MODE_VELOCITY) { - // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); send_velocity_setpoint(); } else if (current_control_mode == CONTROL_MODE_POSITION) { - // RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); send_position_setpoint(); } new_setpoint = false; @@ -404,15 +424,15 @@ private: } } + /** + * @brief Callback function for the attitude topic. + * This function is called every time a new message is received on the topic. + * It only stores the first two measurements while the drone is not armed. + * + * @param msg the message containing the attitude + */ void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { - /* - average q: - - 0.7313786745071411 - - 0.005042835604399443 - - 0.0002370707516092807 - - 0.6819528937339783 - */ if (!armed) { if (base_q_amount > 2) @@ -428,18 +448,22 @@ private: base_q[3] = msg->q[3]; base_q_amount++; } - - // RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); } } + /** + * @brief Callback function for receiving the local position from the Pixhawk + * The local position is only received while the drone is not yet flying (user has not yet taken over control) + * + * @param msg the message conataining the local position + */ void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) { // set start values to current position if (!user_in_control) { // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf - rho_0 = pow(msg->x,2) + pow(msg->y,2); + rho_0 = pow(msg->x, 2) + pow(msg->y, 2); theta_0 = atan2(msg->y, msg->x); last_angle = msg->heading;