How to make it fly in realtime

[EhrazImam]

Hi @beomsu7 i am not getting it even though i am giving it depth and odom topic what else i need to do to make it run in real time. In px4_kino_replan.launch how you are calling px4.launch. Thankyou.

[beomsu7]

The drone need to be set up before launching the px4_kino_replan.launch The drone need to be turned on and mavros need to be running with your drone

[EhrazImam]

The drone need to be set up before launching the px4_kino_replan.launch The drone need to be turned on and mavros need to be running with your drone

Yah! I had done the set up of drone before launching the px4_kino_replan.launch and launched the px4.launch and giving the goal point the issue is the px4_kino_replan.launch is unable to control the drone and giving command to drone..Please help me that how i can do so? and how can give command to drone

[iamrajee]

The drone need to be set up before launching the px4_kino_replan.launch The drone need to be turned on and mavros need to be running with your drone

Do we need to run the VINS estimator seperately? If then we have to change the Odom topic right?

[beomsu7]

@EhrazImam, first, the most difficult point is I don't know your settings of the hw and sw, and mayb your settings and my settings are different. So I gonna explain easily. I use pixhawkboard with px4 and d435 for rgbd camera, t265 for positioning, and jetson board I use mavros to communicate and control the px4 through ROS

The 'px4.launch' is the launch file for running mavros, and in my case, copied the launch file from mavros package for changing some parameters. If you launch the mavros(px4.launch at offboard) then you can check the parameters and also 'control' the drone. At this 'control' part, I publish the /mavros/setpoint_raw/local topic

The kino_replan launch file run the algorithm and publish the setpoint topic, and I converted the setpoint topic to /mavros/setpoint_raw/local, which is running at trajectory_msg_converter_raw.py

So, if you are running every stuff in your offboard, which is connected with pixhawk board, launch the mavros first, and run kino planner. Of course you need to check and change other parameters.

[beomsu7]

@iamrajee Yes, this is just path planner.

[EhrazImam]

@EhrazImam, first, the most difficult point is I don't know your settings of the hw and sw, and mayb your settings and my settings are different. So I gonna explain easily. I use pixhawkboard with px4 and d435 for rgbd camera, t265 for positioning, and jetson board I use mavros to communicate and control the px4 through ROS

The 'px4.launch' is the launch file for running mavros, and in my case, copied the launch file from mavros package for changing some parameters. If you launch the mavros(px4.launch at offboard) then you can check the parameters and also 'control' the drone. At this 'control' part, I publish the /mavros/setpoint_raw/local topic

The kino_replan launch file run the algorithm and publish the setpoint topic, and I converted the setpoint topic to /mavros/setpoint_raw/local, which is running at trajectory_msg_converter_raw.py

So, if you are running every stuff in your offboard, which is connected with pixhawk board, launch the mavros first, and run kino planner. Of course you need to check and change other parameters.

Im getting an error saying [ERROR] [1637157720.610639270]: MODE: Unknown mode: OFFBOARD while giving command rosrun mavros mavsys mode -c OFFBOARD Please help!

[beomsu7]

@EhrazImam If that command is not working, then that's not my part, sorry how about trying 'rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'OFFBOARD'"

[bhaskar-glitch]

@EhrazImam If that command is not working, then that's not my part, sorry how about trying 'rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'OFFBOARD'"

Hi, @beomsu7 Firstly Thanks for the help and support... I'm getting an error actually while switching to OFFBOARD mode the error is Time out! it comes after 3 4 seconds after entering rosrun mavros mavsys mode -c OFFBOARD this. And also have to made any change in px4 files any topic changes in c++ files?? because me is doing the same thing as you have did...Please help because we've made everything work till localisation..and now we're stuck in just launching the drone and make it fly...

[beomsu7]

@bhaskar-glitch Do you mean, you want to switch to offboard mode, but it's not working? If you want to switch to offboard mode, you need to already sending setpoint data

And actually I am not sure, at which point have you got the error Maybe need more detail for helping

[bhaskar-glitch]

@bhaskar-glitch Do you mean, you want to switch to offboard mode, but it's not working? If you want to switch to offboard mode, you need to already sending setpoint data

And actually I am not sure, at which point have you got the error Maybe need more detail for helping

See I'm getting two errors while doing mavsys mode -c OFFBOARD,in the terminal where i launched px4.launch I'm getting error saying UNSUPPORTED FCU and in the terminal where I'm writing mavsys mode -c OFFBOARD this command I'm getting Time out! error. Now the steps I'm doing is:- 1.) Launching the camera and the VINS_FUSION 2.) Launching the Fast planner 3.) Launching the px4.launch (roslaunch mavros px4.launch) 3.) Publishing the topic /mavros/setpoint_raw/local(frame id= map,x=0,y=0,z=2) 4.) Arming the drone

These steps are working perfectly but 5.) While changing the mode to OFFBOARD(mavsys mode -c OFFBOARD) I'm getting an error here.. drone is unable to go to OFFBOARD mode.. I'm also having some doubts:- 1.) Do we have to takeoff my drone with my remote after arming it and then set it to OFFBOARD mode or just we have to arm it and set it to OFFBOARD it'll takeoff by itself ? 2.) Which parameters we've to change in px4 so that by drone will be able to take position inputs from odom?

[beomsu7]

@bhaskar-glitch 1) When the drone is flying, can't switch to offboard mode, so I think your process is right and it should work 2) if you are using external vision based positioning system, mayb you already know, ekf2_aid_mask and also you can check this (http://docs.px4.io/master/en/computer_vision/visual_inertial_odometry.html)

You mean from first step to arming step, it works, then mavros is working Then I recomend check 1) Is the local position is working well with vins? With hand helding test, check the positioning 2) Check the px4 with qgc(q ground control) log, , qgc gonna be display information more then terminal

[bhaskar-glitch]

@bhaskar-glitch

1. When the drone is flying, can't switch to offboard mode, so I think your process is right and it should work
2. if you are using external vision based positioning system, mayb you already know, ekf2_aid_mask and also you can check this (http://docs.px4.io/master/en/computer_vision/visual_inertial_odometry.html)

You mean from first step to arming step, it works, then mavros is working Then I recomend check

1. Is the local position is working well with vins? With hand helding test, check the positioning
2. Check the px4 with qgc(q ground control) log, , qgc gonna be display information more then terminal

In (http://docs.px4.io/master/en/computer_vision/visual_inertial_odometry.html) it is written to check some points and that I've done but some more points(attached screenshot of it) are being checked by itself. Is there any problem with it?

Yah! when I'm doing echo of topic /mavros/local_position/odom** I'm receiving the odom data..Is this the local position you are talking about or any other topic should show the odom data... Can you please share me you px4.launch file..do you have made any changes except the port and baudrate of pixhawk ?....have you added any topic in px4.launch** file??

[beomsu7]

@bhaskar-glitch yes that topic is right In my case I used 280 ekf2_aid_mask with t265 There's nothing I changed in px4.launch And did you check the qgc log when you try to switch to offboard mode? And now I can't check my pc

[bhaskar-glitch]

@bhaskar-glitch yes that topic is right In my case I used 280 ekf2_aid_mask with t265 There's nothing I changed in px4.launch And did you check the qgc log when you try to switch to offboard mode? And now I can't check my pc

Yah I've checked the qgc log but nothing related to odometry is their and Qgc log means the log error file right? or you are talking about the MAVLINK INSPECTOR in qgc? If you are talking about the MAVLINK INSPECTOR then nothing related to ODOMETRY data is being received their.

[beomsu7]

@bhaskar-glitch This vehicle message, I mean

[bhaskar-glitch]

@bhaskar-glitch This vehicle message, I mean

While switching to OFFBOARD the error says UNSUPPORTED MODE. Are you using mavros_bridge ?? Because I'm using mavros_bridge and also made some changes in launch of my odom topic..! The changes I've made:- I.) launched my D435i camera in the place of t265.launch II.) also remaped the /camera/sample_throttled topic with my odom(VINS_FUSION) topic i.e. /mavros/local_position/odom

[bhaskar-glitch]

Hi @beomsu7, I'm attaching my rqt graph and rqt tf tree so that you'll get an idea about the topic we're getting rqt_tf_tree rqt_graph these are the frames and the topics being used and published in my case..Please let me know if anyone of the topic is missing or if we need to add something

[beomsu7]

@bhaskar-glitch I'm not using that mavros bridge I use pixhawk4 and jetson xavier and wired these two stuff

[bhaskar-glitch]

@bhaskar-glitch I'm not using that mavros bridge I use pixhawk4 and jetson xavier and wired these two stuff

Seriously :( Like,I'm doing each and everything like you did,just I've added the VIO(VINS_FUSION) stuff because I'm using d453i camera and then also my drone is unable to fly.... ;(

[bhaskar-glitch]

@bhaskar-glitch I'm not using that mavros bridge I use pixhawk4 and jetson xavier and wired these two stuff

As you said earlier that you are publishing /mavros/setpoint_raw/local so can you share me that exactly how you are doing that ?? Also which pins you have used to connect your pixhawk4 with xavier ?(I'm using GND,TX,RX,CTS,RTS pins) what about yours??

[beomsu7]

@bhaskar-glitch I'm using UART1_TX and RX(dev/ttyTHS0)

and mayb this one is the picture I screenshoted

[bhaskar-glitch]

/mavros/setpoint_raw/local

Thank you so much! btw what about the topic /mavros/setpoint_raw/local how it's being published

[beomsu7]

@bhaskar-glitch it is almost same with mavros/setpoint_position/local

[bhaskar-glitch]

@bhaskar-glitch it is almost same with mavros/setpoint_position/local

See my drone is taking yaw just after taking off.So I want to know the param you are giving in the topic while publishing My cpp to switch it to offboard is :

include <ros/ros.h>

include <geometry_msgs/PoseStamped.h>

include <mavros_msgs/CommandBool.h>

include <mavros_msgs/SetMode.h>

include <mavros_msgs/State.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>
        ("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("mavros/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 = 1;

//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();

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();
        }
    }

    local_pos_pub.publish(pose);

    ros::spinOnce();
    rate.sleep();
}

return 0;

}

[bhaskar-glitch]

@bhaskar-glitch it is almost same with mavros/setpoint_position/local

Btw can you tell me the grid size in real world..like if I'm getting 40*40 grid on rviz in fast planner...what is it's real value in meter??

[iamrajee]

Btw can you tell me the grid size in real world..like if I'm getting 40*40 grid on rviz in fast planner...what is it's real value in meter??

1 Unit in Rviz = 1 meter. (By default, unit for distance in ROS is meter)

[bhaskar-glitch]

Btw can you tell me the grid size in real world..like if I'm getting 40*40 grid on rviz in fast planner...what is it's real value in meter??

1 Unit in Rviz = 1 meter. (By default, unit for distance in ROS is meter)

Oh ok! Thanks:)

[bhaskar-glitch]

Hi @beomsu7, The Hardwares i am using:

• Pixhawk 2.4.8(firmware px4)
• Jetson xavier nx
• D435i The Algorithms i'm using:
• Vins Fusion for localisation(output-good)
• Fast-Planner We have taken some flights today with fast planner but we are facing some of the issues:- I.)When I'm giving the goal point with 2d nav goal the drone starts moving forward and up both at a same time and the upward height it is taking is almost 3meter...but I'm taking it off at only 1meter height and giving the goal point...why it is moving upward.. ii.) When I'm giving the goal point the drone is taking yaw of 90degrees...why??is this written in the code to take yaw of some degrees Currently I'm not taking off using OFFBOARD mode because when I was giving the z (axis value = 1)in topic /mavros/setpoint_raw/local it was moving too aggressively upward and its continuously trying to move upward and I guess it is trying to achieve some point have you set any point in the code?? PLease help

[beomsu7]

@bhaskar-glitch sorry for late reply Is that code is from mavros offboard example at docs.px4?

In my case, when I use real drone, I arm the drone and switch to offboard mode with rc controller

I think you need to check and compare the /mavros/setpoin_raw/local and /mavros/local_position/pose and how much fps you get from vins with xavier nx?

[bhaskar-glitch]

@bhaskar-glitch sorry for late reply Is that code is from mavros offboard example at docs.px4?

In my case, when I use real drone, I arm the drone and switch to offboard mode with rc controller

I think you need to check and compare the /mavros/setpoin_raw/local and /mavros/local_position/pose and how much fps you get from vins with xavier nx?

I'm getting 30fps from vins with xavier nx

[bhaskar-glitch]

Hi @beomsu7, When I'm launching the fast planner I'm getting origin_: -20 -10 -1 printed on my screen. Is this the origin point of the drone? DO I have to change it according to my origin coordinate?? if yes then when exactly do I've to make changes?? Please help!!!

[beomsu7]

@bhaskar-glitch do you mean when you launch the fast planner, then your drone's /mavros/local_position/pose value is xyz -20 -10 -1? that's strange and little bit dangerous Fast planner is local planner and not affect to px4

[bhaskar-glitch]

@bhaskar-glitch do you mean when you launch the fast planner, then your drone's /mavros/local_position/pose value is xyz -20 -10 -1? that's strange and little bit dangerous Fast planner is local planner and not affect to px4

Actually I don't know is it /mavros/localposition/pose or something else but whenever I launch fast planner it starts by printing some values like grid size:*4040 and origin: -20 -10 -1* and like grid size is 4040 that's correct I able to see that on rviz but what about the origin point??

[beomsu7]

@bhaskar-glitch At that part that grid size is different with rviz's grid, tha grid size 40*40 is from https://github.com/beomsu7/Fast-Planner/blob/px4-version(sitl-and-real)/fast_planner/plan_manage/launch/px4_kino_replan.launch#L3 Also check this one https://github.com/beomsu7/Fast-Planner/blob/px4-version(sitl-and-real)/fast_planner/plan_env/src/sdf_map.cpp And can you show me that? Actually I've not seen that kinds of thing

[bhaskar-glitch]

@bhaskar-glitch At that part that grid size is different with rviz's grid, tha grid size 40*40 is from https://github.com/beomsu7/Fast-Planner/blob/px4-version(sitl-and-real)/fast_planner/plan_manage/launch/px4_kino_replan.launch#L3 Also check this one https://github.com/beomsu7/Fast-Planner/blob/px4-version(sitl-and-real)/fast_planner/plan_env/src/sdf_map.cpp And can you show me that? Actually I've not seen that kinds of thing

Yah sure I'll share uh the videos. Btw what is your frame of fast planner when u implemented it on hardware and make it fly.Is that map?? because my frame of the fast planner is map and the vins fusion which I'm using for localization is on the world frame...Is this a problem for not having both on the same frame????

[beomsu7]

@bhaskar-glitch Aha mayb that point can be the problem, then just change and fit the frame

[bhaskar-glitch]

@bhaskar-glitch Aha mayb that point can be the problem, then just change and fit the frame

I'm asking that at what frame u are running your fast planner ??

[beomsu7]

@bhaskar-glitch every high level task is running on 'map' frame, in my case

[bhaskar-glitch]

@bhaskar-glitch every high level task is running on 'map' frame, in my case

Hi @beomsu7, Right now what I'm facing is that my point cloud is not moving along with the drone on the grid as the frame id of pointcloud(sdf_occupancy_inflate) is map so I guess that's the issue..Now what about your frame id of pointcloud(sdf_occupancy_inflate) and if your frame id isn't map then in which file I've to make changes to change my frame id?? I've tried by making changes in sdf_map.cpp but nothing helped...

[beomsu7]

@bhaskar-glitch check your rqt_tf_tree and make the correct tf tree

[bhaskar-glitch]

Hi @beomsu7, Finally the fast planner is working fine,the path planning is also fine but one issue we're facing is that our point cloud from topic(/sdf/occupancy_inflate) from realsense topic(/camera/depth/image_raw) is quite slow and as the sdf map is slow in comparison to real-world it may cause damge to drone and the delay is 8/9 sec.

Btw my publishing rate of topics are:

/sdf_map/occupancy_inflate

/camera/depth/image_rect_raw

PLEASE HELP

[beomsu7]

@bhaskar-glitch Are you running fastplanner at your remote pc? or onboard pc In my case, whey I ran planner on remote pc, there was an huge delay like you

[bhaskar-glitch]

@bhaskar-glitch Are you running fastplanner at your remote pc? or onboard pc In my case, whey I ran planner on remote pc, there was an huge delay like you

I'm running fast planner obviously in my remote pc when i use to go for testing but i tested the pointcloud on the onboard pc too...the delay was same there

[beomsu7]

@bhaskar-glitch about that parts, I have no idae, sorry

[bhaskar-glitch]

@bhaskar-glitch I'm using UART1_TX and RX(dev/ttyTHS0)

and mayb this one is the picture I screenshoted

Hi @beomsu7 as in your rqt_graph there is a topic /mavros/odometry/out which is being subscribed by /mavros..So whats the publish rate of topic /mavros/odometry/out ?? and also what's the publish rate of /mavros/local_position/pose and /mavros/local_position/odom..In my case it's around 0.5.

[bhaskar-glitch]

Hi, @beomsu7, I'm facing a problem while using odometry for the position of the drone..I'm using realsense d435i, jetson xavier nx, and pixhawk 2.4.8 with px4 firmware....I'm using a bridge VIO to provide odometry data to px4..how the issue I'm facing right now is that bridge is sending data to /mavros/odometry/out and then odometry out is sending those data to mavros and further those data get into /mavros/local_position/odom and pose...but the position data (X,y,z) from the VINS(odometry)is correct till /mavros/odometry/out(topic) for after that when I'm doing rostopic echo of /mavros/local_positon/odom and pose it is giving some random wrong values...I don't know why because at /odometry/out topic values are very fine but after going to mavros node it gets wrong!! also, I changed all the parameters which is required for vision pose estimate like(EKF2_AID_MASK=24, EKF2_HGT_MODE=VISION and turned off the barometer)!! Please help regarding the issue that why I'm getting that random wrong values.

ROSGRAPH