Hi i’m new to mavros, and I’m trying to use mavros’s setpoint_position command. I understand it is a translation of mavlink’s SET_POSITION_TARGET_LOCAL_NED. I cam across PX4’s tutorial on how to use it, but I can’t seem to for the life me get it to work on APM. I am testing using SITL with gazebo. Here is my code
/**
* @file nedTest.cpp
*/
I guess it is also worth noting that the result of the above code is that the drone arms, the blades spin up then it doesn’t move, and the drone is dis armed after around 10 seconds. this is the output from the debugger console
@rmackay9 solution is correct. I forgot that I opened this thread, so I never posted the solution.
The mavros code for takeoff request is as follows just in case other people stumble upon this in the future.
HAI,
I have been trying to communicate with pixhawk using mavros and used the same code as mentioned in this thread. But my motors are not spinning at all even though the mavros terminal shows that the copter is armed. I also tried using Takeoff command as mentioned in this thread.
I guess I am missing something simple! Any help will be appreciated! @rmackay9 could you please take a look at this too!
My set up is Pxhawk with Arducopter V3.6.9 connected by telem 2 port to Raspberry Pi. Mavros commands send via ROS kinetic installed in RPi.
Thanks in advance!
/**
@file drone_nav_node.cpp
Offboard control(control of drone from a companion computer) example node, written with MAVROS for PX4 Flight Stack
to be run on RPi with ROS kinetic installed
*/
//initially include all header files necessary for compiling the subscribers, publishers and services
//which are going to be used in the code
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
//creating a callback 'state_cb' to store the current state of the copter in variable 'current_state'
//this will be called by subscriber 'state_sub' when a message arrives in the 'mavros/state' topic
//which is of data type 'mavros_msgs/State'
mavros_msgs::State current_state; //this sets the variable type for 'current_state'
//variable
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
//the main ros function with all subscribers, publishers and services
int main(int argc, char **argv)
{
ros::init(argc, argv, "drone_nav_node"); //initialising ros, with node name 'offn_node'
ros::NodeHandle nh; //main accesspoint for this node to communicate with the system
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> //subscriber 'state_sub' is subscribing to topic 'mavros/state of type mavros_msgs/State by calling state_cb
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> //publisher 'local_pos_pub' topic 'maros/.../local' type 'geo../PoseStamped'
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> //serviceclient 'arming_client' type(header) 'mav../Com..Bool' service 'mavros/cmd/arming'
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> //serviceclient 'set_mode_client' type(header) 'mav../SetMode' service 'mavros/set_mode'
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz(=>1cycle/0.5sec); since pxhawk has a
//timeout of 0.5sec
ros::Rate rate(20.0); //here we give 20Hz
// wait for FCU connection, as soon as the bool 'cuttent_state.connected' becomes true while loop
//will be exited(!true=false)
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
ROS_INFO("Connection with FCU has been establised.");
//initialising pose variable of datatype geo../PoseStamped and assigning (from mavros doc
//pose.position.)x,y & z values in ros framesystem(ENU)
//this will be converted to pixhawk NED frame by mavros
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0.0;
pose.pose.position.y = 0.0;
pose.pose.position.z = 2.0;
//send a few setpoints before starting, here we send 100 times
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose); //calling the publisher 'local_pos_pub' to publish the pose
ros::spinOnce();
rate.sleep(); //at the rate of 20Hz
}
//initialise/define variable of type(header) mavros_msgs/SetMode to set modes of the copter
//this variable will be called by the serviceclient 'set_mode_client'
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "GUIDED_NOGPS"; //OFFBOARD node since we are
//using px4 flight stack
offb_set_mode.request.base_mode = 0.0;
//initialise variables of type mavros_msgs/CommandBool to arm&disarm the copter
//thes variables will be called by the serviceclient 'arming_client'
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true; //true=> armig the copter
mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false; //fals=> disarmig the copter
//initialise a variable to store the current time (used below to have a control over the time gap
//between requests/commands send to the pixhawk from RPi)
ros::Time last_request = ros::Time::now();
//making 5sec delay between adjacscent requests
//first sets the mode of the copter by calling seviceclient 'set_mode_client' and also make sure it is
//sent by using service response - bool 'response.mode_sent'
//then arms the copter by calling service 'arming_client' and also make sure this was
//communicated successfully using service response - bool 'response.success'
//after that keep sending the pose(set_points) to the drone
while(ros::ok() && !current_state.armed){
if( current_state.mode != "GUIDED_NOGPS" &&
(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("GUIDED_NOGPS 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();
}
}
local_pos_pub.publish(pose);
ROS_INFO("Startposition z=%f", pose.pose.position.z);
ros::spinOnce();
rate.sleep();
}
last_request = ros::Time::now();
//new takeoff command
//request takeoff
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
("/mavros/cmd/takeoff");
mavros_msgs::CommandTOL srv_takeoff;
srv_takeoff.request.altitude = 1.5;
if(takeoff_cl.call(srv_takeoff)){
ROS_INFO("takeoff sent %d", srv_takeoff.response.success);
}
else{
ROS_ERROR("Failed Takeoff");
return -1;
}
//after 5sec giving command to drone to takeoff to an altitude of 1.5m
while(current_state.armed){
if( ros::Time::now() - last_request > ros::Duration(5.0)){
ROS_INFO("Initiating Takeoff");
pose.pose.position.z = 1.5;
for(int i = 0; ros::ok() && i < 10*20; ++i){ // sending altitude command continuously for around 3.33min(200sec)
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
ROS_INFO("Takeoff done to an altitude of z=%f !", pose.pose.position.z);
}
}
I have made some tutorials using Ardupilot + Mavros as well as some helper functions to handle a lot of the extra code associated with various navigation tasks including transforming reference frames.
this code executes a simple square pattern, and the link bellow is the tutorial for the program, with the link to the associated youtube video
if you want to take a look at the underlying code of the functions in my program take a look at
also, if all of your code is correct and works in a simulator, I would check your GCS for any failsafes. if not it could also be that mavros isn’t publish messages at the correct rate. Not sure why this happens, but it does sometime on companion computer fcc links. you can run the command listed in this forum on your companion computer to fix the issue. https://answers.ros.org/question/202593/mavros-with-serial-connection/
just needed to make sure one thing which I missed to ask, here is an image of my mavros terminal, it shows ‘requesting home position’, is it normal when we initialize mavros apm.launch?
Are you by chance running your drone without gps? It seems like drone may not be able to set the ekf origin, thus cannot generate a home position. This would also lead to the drone not being armable
Hai thanks again for your response!
Yes, actually I am planning to do autonomous navigation in GUIDED_NOGPS mode. So how can I set my parameters to proceed in this direction with out GPS. I had an interesting experience just a few hours ago i flipped the order of execution and armed the drone first then asked it to switch to GUIDED_NOGPS mode. then the Mavros terminal showed “Pre Arm: throttle too high”, I just tried reducing the throttle in my RC and then my drone armed and motor started spinning!
but still it doesnt seem like a proper way to arm the drone!
If you have any info in this direction please let me know.
Note: I have been testing this without propellers to see how all the set up works out. and also I have an Optical flow camera and teraone ranger in the bag to be integrated before I get it flying with propellers.(Besides it possible to skip using optical flow camera if i am using an indoor beacon to provide position in nogps mode?)
It is going to be very difficult to do gps denied navigation in “guided no gps” mode. I have done a very similar architecture in the past pixflow + lidar lite for gps denied navigation. To make this work my friends and I use the “guided” flight mode. While it says it needs gps, it actually just needs a 3d fix. This can be created by providing an ekf origin and using an optical flow sensor with EKF_GPS_TYPE = 3. The ekf origin can be set in 2 different ways. 1 way is to right click on the desired origin on the mission planner map and click set ekf origin. The other way is programmatically via mavros. below is the link to some code my friend wrote to provide ardupilot with a gps origin of 0,0,0(lat, lon, alt) .
there might be some dead code in here (not sure), but it works (used it in a couple other drone projects after early 2018).
But in summary, use mode “guided” it will allow you to use all the position loops already in ardupilot, as “guided no gps” only accepts attitude and thrust commands.
also, here is a bash script my team used to start all the code on our companion computer. it might be helpful down the road.
I really wanted to make more youtube videos on this stuff, but ya know. life comes at you fast
I also just remembered that, the mavros set ekf origin method will only work if you remove the lat lon check for 0,0 in autopilot. The other option is to not set the ekf origin to 0,0, but some other coordinates. Here is the code change my team did.
Thank you verymuch for your response !
I was on a vacation, and prior to that i was struggling to get the blades spinning and was not able to set ekf origin via missionplanner. Can we do this process using MissionPlanner without the codes. I mean setting EKF origin via MP map and remove lat, lng check also ?
Since I was stuck in getting EKF origin via MP, i tried to see Guided_nogps with thrus and attitude commands. I was able to give thrust command and the blades were spinning. But couldnt find mavros commands to give roll, pitch and yaw. Also when I conect px4flow it serches for origin even when GPS/EKF is disabled. Got stuck there too !
Actually my aim is to control my drone using Rpi and mavros to make the drone take off in stabilise mode to a specific altitude, then
either use Guided_no gps mode to move drone left, rigth and turn using attitude&thrust commnds - stuck here with issue of not able to
or use Guided mode to move drone left, rigth and turn by giving position commnads
both are fine since i am working indoors and not gonna do a path planning, just need to move aroung in a 1.5mx1.5m area.
Below is the results I got and the parameter settings for the 2 modes Guided_nogps and guided.
If anyone can provide an insight in getting this to work in any of this mode will be of great help, thanks in Advance!
Guided nogps trial
Optical flow - showing values
Drone not arming, showing prearm nav check error!
Guided mode trial
Optical flow not working
EKF set home here not working
Drone armed but with EKF but with EKFvariance error and when switched to guided system got stuck.
Sorry for the late response. I was going to respond to this last week, but I forgot. The guided method is the route to go imo. The optical flow should be giving values, make sure all of the parameters for the optical flow are set correctly.
a couple of things I have learned from experience is
1.) the proper navigation of your drone is directly related to the health of your EKF position and orientation estimates. One thing that I had to do to get my drone to work, was to lower the weighting ekf weighting of the compass, but this doesn’t seem to be your problem yet. The compass indoors is heavily effected by the emi of the building. This can vary from day to day. Ultimately your compass is a very bad estimator of your orientation indoors. The weighting of the compass can be adjusted with this parameter: ek2_yaw_m_nse https://ardupilot.org/copter/docs/parameters.html#ek2-yaw-m-nse-yaw-measurement-noise-rad
2.) use the ekf status box in mission planner to figure out why your ekf is upset. You can see this by double clicking on the word EKF in the hud
3.) make sure you are flying over a textured surface. without a textured surface the optical flow will not be able to give you correct flow estimates.
4.) look at the messages from the drone when a failsafe occurs. this can be seen here in mission planner.
5.) setting up a simulator can help you debug a lot of these issues without having to risk your drone, and a lot of times can be done way faster. I would encourage you to setup the gazebo sim and simulate your mission before you start doing tons of flight tests. I have some instructions on my github.
