sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

change to acceleration setpoints

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-02 13:55:06 +02:00
parent dd474bf8ea
commit b971c7989e
2 changed files with 27 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/px4_connection/src/heartbeat.cpp
View File

@@ -42,8 +42,8 @@ private:
auto msg = px4_msgs::msg::OffboardControlMode(); auto msg = px4_msgs::msg::OffboardControlMode();
msg.position = false; msg.position = false;
msg.velocity = false; msg.velocity = false;
msg.acceleration = false; msg.acceleration = true;
msg.attitude = true; msg.attitude = false;
msg.body_rate = false; msg.body_rate = false;
msg.actuator = false; msg.actuator = false;

49
src/px4_connection/src/px4_controller.cpp
View File

@@ -39,7 +39,7 @@ public:
trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10); trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
// offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10); // offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); set_attitude_service_ = this->create_service<drone_services::srv::SetVelocity>("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
// create timer to send vehicle attitude setpoints every second // create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this));
@@ -52,7 +52,7 @@ private:
rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher;
rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_; rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
rclcpp::Service<drone_services::srv::SetAttitude>::SharedPtr set_attitude_service_; rclcpp::Service<drone_services::srv::SetVelocity>::SharedPtr set_attitude_service_;
// rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_; // rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
@@ -67,20 +67,21 @@ private:
float cur_yaw = 0; float cur_yaw = 0;
float last_setpoint[3] = {0,0,0}; float last_setpoint[3] = {0,0,0};
float last_angle = 0;
float last_thrust = 0; float last_thrust = 0;
void set_setpoint( void set_setpoint(
const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::SetAttitude::Request> request, const std::shared_ptr<drone_services::srv::SetVelocity::Request> request,
const std::shared_ptr<drone_services::srv::SetAttitude::Response> response) const std::shared_ptr<drone_services::srv::SetVelocity::Response> response)
{ {
last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[0] = request->x_speed;
last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[1] = request->y_speed;
last_setpoint[2] = degrees_to_radians(request->roll); last_setpoint[2] = request->z_speed;
last_thrust = request->thrust; last_angle = request->angle;
RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_thrust); RCLCPP_INFO(this->get_logger(), "New setpoint: x:%f y:%f z:%f angle:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_angle);
response->status = 0; response->status = 0;
} }
@@ -91,9 +92,9 @@ private:
auto msg = px4_msgs::msg::TrajectorySetpoint(); auto msg = px4_msgs::msg::TrajectorySetpoint();
if (this->get_clock()->now().seconds() - start_time_ < 10) if (this->get_clock()->now().seconds() - start_time_ < 10)
{ {
msg.velocity[0] = 0; msg.acceleration[0] = 0;
msg.velocity[1] = 0; msg.acceleration[1] = 0;
msg.velocity[2] = D_SPEED(10); msg.acceleration[2] = D_SPEED(10);
msg.yawspeed = 0; msg.yawspeed = 0;
msg.yaw = -3.14; msg.yaw = -3.14;
} }
@@ -105,9 +106,9 @@ private:
has_swithed = true; has_swithed = true;
} }
msg.velocity[0] = last_setpoint[0]; msg.acceleration[0] = last_setpoint[0];
msg.velocity[1] = last_setpoint[1]; msg.acceleration[1] = last_setpoint[1];
msg.velocity[2] = D_SPEED(last_setpoint[2]); msg.acceleration[2] = D_SPEED(last_setpoint[2]);
msg.yawspeed = 0.5; msg.yawspeed = 0.5;
} }
@@ -136,15 +137,15 @@ private:
calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]);
} }
else // else
{ // {
if (flying) // if (flying)
{ // {
publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); // publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
flying = false; // flying = false;
RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); // RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds");
} // }
} // }
// set quaternion // set quaternion