Problem Statement
- Sending 30 m/s velocity to an IRIS drone on SITL simulation in OFFBOARD mode but the ground speed is still shown 9.5 m/s in QGroundControl.
- Using Mavros, I am sending 30m/s to the
setpoint_raw/local
topic.
Setup
- Simulation setup: SITL - PX4 Gazebo
- Firmware: 1.10.1
- Mode: OFFBOARD
- MAVROS
- MPC_XY_VEL_MAX: 20m/s
Steps to reproduce
- Start SITL simulation on gazebo.
- Run the script shown below that sends 30m/s to the
PositionTarget
message type.
More information
Speed shown in QGroundControl:
Speed published:
Code
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro
* Flight Stack and tested in Gazebo SITL
*/
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <ros/ros.h>
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr &msg) { current_state = *msg; }
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
ros::Subscriber state_sub =
nh.subscribe<mavros_msgs::State>("robot100/sitl/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>(
"robot100/sitl/setpoint_position/local", 10);
ros::Publisher local_vel_pub =
nh.advertise<mavros_msgs::PositionTarget>("robot100/sitl/setpoint_raw/local", 10);
ros::ServiceClient arming_client =
nh.serviceClient<mavros_msgs::CommandBool>("robot100/sitl/cmd/arming");
ros::ServiceClient set_mode_client =
nh.serviceClient<mavros_msgs::SetMode>("robot100/sitl/set_mode");
// the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while (ros::ok() && !current_state.connected)
{
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
mavros_msgs::PositionTarget setpoint_msg;
setpoint_msg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
setpoint_msg.type_mask = 0b0000011111000111;
setpoint_msg.position.x = 0;
setpoint_msg.position.y = 0;
setpoint_msg.position.z = 0;
setpoint_msg.velocity.x = 30;
setpoint_msg.velocity.y = 0;
setpoint_msg.velocity.z = 0;
setpoint_msg.yaw_rate = 0;
// send a few setpoints before starting
for (int i = 100; ros::ok() && i > 0; --i)
{
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
bool takeoff;
takeoff = false;
while (ros::ok())
{
if (current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0)))
{
if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
{
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
}
else
{
if (!current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0)))
{
if (arming_client.call(arm_cmd) && arm_cmd.response.success)
{
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
if (!takeoff)
{
for (int i = 0; i < 10; i++)
{
local_pos_pub.publish(pose);
}
takeoff = true;
}
setpoint_msg.header.stamp = ros::Time::now();
local_vel_pub.publish(setpoint_msg);
ros::spinOnce();
rate.sleep();
}
return 0;
}