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

add hover after 20 s

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-01 12:43:02 +02:00
parent 04cad041b6
commit 3f2ffd6dc7
1 changed files with 18 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -58,11 +58,25 @@ private:
void send_trajectory_setpoint() void send_trajectory_setpoint()
{ {
auto msg = px4_msgs::msg::TrajectorySetpoint(); auto msg = px4_msgs::msg::TrajectorySetpoint();
msg.velocity[0] = 0; if (start_time_ - this->get_clock().now().seconds() < 20)
msg.velocity[1] = 0; {
msg.velocity[2] = -15; msg.velocity[0] = 0;
msg.yawspeed = 0; msg.velocity[1] = 0;
msg.velocity[2] = -15;
msg.yawspeed = 0;
} else {
if (!has_swithed)
{
RCLCPP_INFO(this->get_logger(), "switching to 0 vel");
has_swithed = true;
}
msg.velocity[0] = 0;
msg.velocity[1] = 0;
msg.velocity[2] = 0;
msg.yawspeed = 0;
}
// if (setpoint_count < 30) // if (setpoint_count < 30)
// { // {