@Vrinda You need to publish to setpoint_attitude/attitude
Hello @Jaeyoung-Lim !
Merry Christmas ! Thank you very much for helping me and everyone on the community. I am following your tutorial here, however when I run the node, I do not see any output. The console just keeps on saying “Offboard enabled” repeatedly.
@Vrinda Do you mean that you modify the example into publishing attitude setpoints?
For the console output there are.a few things that could have gone wrong. Unfortunately the enabled output does not mean that the node talked to the flight controller but that it published a service to enable off board.
You should check
- if the topics are being pulbished properly
2.if the flight controller is actually receiving the topics you are publishing
and merry Christmas to you too Thanks
1 Like
Hello @Jaeyoung-Lim !
I modified to example mentioned on dev guide as well as I tried your example on 404 warehouse. I don’t see the output on gazebo. Like I am just giving a setpoint for a constant pitch.
- I checked it and all topics are published
- What should I do here ? I see that mavros says…WP mission received …etc
Please find my code below for your reference. I want to pitch with 0.1 radians, I have converted that to a quaternion. The drone does not pitch in gazebo. Rather it becomes unstable. I feel it is trying to control it’s position and attitude to that of mavros/setpoint_position/local
/**
-
@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
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::Publisher pub_att = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_attitude/attitude",10);
ros::Publisher pub_thr = nh.advertise<std_msgs::Float64>("/mavros/setpoint_attitude/att_throttle", 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 = 2;geometry_msgs::PoseStamped cmd_att;
std_msgs::Float64 cmd_thr;//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”);
while(ros::ok()){
cmd_att.header.stamp = ros::Time::now();
cmd_att.header.seq=count;
cmd_att.header.frame_id = 1;
cmd_att.pose.position.x = 0.0;//0.001some_object.position_x;
cmd_att.pose.position.y = 0.0;//0.001some_object.position_y;
cmd_att.pose.position.z = 0.0;//0.001*some_object.position_z;cmd_att.pose.orientation.x = 0; cmd_att.pose.orientation.y = 0.04; cmd_att.pose.orientation.z = 0; cmd_att.pose.orientation.w = 1; cmd_thr.data = 0.3; pub_att.publish(cmd_att); pub_thr.publish(cmd_thr); local_pos_pub.publish(pose); count++; ros::spinOnce(); rate.sleep(); } } last_request = ros::Time::now(); } } pub_att.publish(cmd_att); pub_thr.publish(cmd_thr); local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep();}
return 0;
what do you mean that it does not pitch and becomes unstable? could you clarify the behavior?
your quaternion is not a unit quaternion(the norm of the quaternion is larger than 1), which I am not sure what the impact is
1 Like
It goes in some random direction. Probably I need to normalize the quaternion. I will do that and keep you posted. Thanks 
I have the topics published. However the actual system does not follow the setpoint.
Setpoint :
orientation:
x: 0.0
y: 0.0797452222829
z: 0.0
w: 0.996815278536
Actual imu data:
x: 0.867826665165
y: -0.470006018113
z: 0.13514842161
w: 0.0877851419524
x: 0.867666507141
y: -0.470329453028
z: 0.135104973477
w: 0.0877028871092
x: 0.867364314237
y: -0.470905347566
z: 0.135014373558
w: 0.0877416042579
Hello @Jaeyoung-Lim
I want to achieve position control, but after my drone reaches the setpoint it stays there for half a second and then continues to go up. Can you help me find the reason or find a way to keep it in one position?
Any help is appreciated. Thank you
I have opened a topic also : EKF and Motion capture Offboard flight
@shortcut21 please don’t hijack this thread which is for attitude control using ROS and create a separate thread instead.
@Vrinda Your actual imu data is not helpful as it doesn’t contain any information on the vehicle state. It would be more helpful if you can provide a log of the vehicle flying away.
Assuming that the imu data shows the initial attitude, looks like your aircraft is not alined to the inertial frame. This means that your desired orientation will not just make the drone pitch but also change attitude in various axises to reach the desired attitude.
I hope this is helpful
Hello @Jaeyoung-Lim !
I have been trying two methods,
- Publish to “/mavros/setpoint_attitude/attitude” and “/mavros/setpoint_attitude/att_throttle”
- Publish attitude sp and thrust sp to “/mavros/setpoint_raw/attitude” (I can give thrust as well as attitude sp)
Method 1:
Code:
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, “att1”);
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 100, state_cb);
ros::Publisher pub_att = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_attitude/attitude",100);
ros::Publisher pub_thr = nh.advertise<std_msgs::Float64>("/mavros/setpoint_attitude/att_throttle", 100);
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(100.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped cmd_att;
cmd_att.header.stamp = ros::Time::now();
cmd_att.header.seq=count;
cmd_att.header.frame_id = 1;
cmd_att.pose.position.x = 0.0;//0.001*some_object.position_x;
cmd_att.pose.position.y = 0.0;//0.001*some_object.position_y;
cmd_att.pose.position.z = 0.0;//0.001*some_object.position_z;
double mag = sqrt(0.08*0.08 + 1);
cmd_att.pose.orientation.x = 0;
cmd_att.pose.orientation.y = 0.08/mag;
cmd_att.pose.orientation.z = 0;
cmd_att.pose.orientation.w = 1;
std_msgs::Float64 cmd_thr;
cmd_thr.data = 0.3;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
pub_att.publish(cmd_att);
pub_thr.publish(cmd_thr);
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();
}
}
pub_att.publish(cmd_att);
pub_thr.publish(cmd_thr);
ros::spinOnce();
rate.sleep();
}
return 0;
}
Output :
_[ INF_O] [1546028867.071798298]: Connected to master at [localhost:11311]
[ INFO] [1546028881.132367033]: Offboard enabled
[ INFO] [1546028886.140303814]: Offboard enabled
[ INFO] [1546028891.151608395]: Offboard enabled
Only when I publish a position setpoint as mentioned in the dev guide (along with attitude sp), the code runs successfully, else it gives Offboard enabled repeatedly as shown above. Do you know what is happening here ? (Please note, earlier I was publishing a position setpoint in Z direction along with attitude setpoint. I got bad response, so I decided to do one thing at one time. Thus, am only publishing att sp. I will provide the logs once successful). Currently am working on Method 2.
Thanks much !
Apologies ! Even after doing this I get the same result for Method 1 @Jaeyoung-Lim !
double mag = sqrt(0.08*0.08 + 1);
cmd_att.pose.orientation.x = 0;
cmd_att.pose.orientation.y = 0.08/mag;
cmd_att.pose.orientation.z = 0;
cmd_att.pose.orientation.w = 1/mag;
I got the Method 2 working. I will post the code when I get chance. I observed from QGC that all the angles match their respective setpoints. Additionally I also performed HITL with attitude setpoint and it works. I was just curious to know why Method 1 fails ?
My second query is the frequency issue. I am using a sensor to provide attitude setpoints. However it is giving data at 80 Hz. I read from few posts that minimum frequency should be 100 Hz for attitude offboard control. Will 80 Hz be fine ? (It works with HITL, am not sure if it will work when I start flying). Thank you !
Hello @desmond13 !
I do not have the code at this moment. However, it is similar to the one indicated above with only change in Publisher to /mavros/setpoint_raw/attitude. Let me know if you have further queries.
@desmond13 The geometric_controller in this repo can be a good reference for you on sending /mavros/setpoint_raw/attitude
2 Likes
Hello @Vrinda !
Could I ask you to share your code with us if you now finished it? It would be helping me a lot.
Thank you in advance!
1 Like