ROS_PX4_gazebo学习记录

在官方程序上（PX4 wiki上为offboard起飞到2m高度）进行更改，实现首先起飞到固定点（x=1,y=2,z=5), 然后按照给定角度飞行。

补充：最终实现效果链接为

gazebo_mavros_PX4 大倾角测试
PX4_gazebo_mavros 自旋翻测试
PX4_ROS_gazebo多机仿真（画圆）

说明：新建世界模型world，首先从gazebo中添加想要的元素，然后另存为world，最后在world模型中修改物理属性标签内的内容如下：

<physics name='default_physics' default='0' type='ode'><gravity>0 0 -9.8066</gravity><ode><solver><type>quick</type><iters>10</iters><sor>1.3</sor><use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling></solver><constraints><cfm>0</cfm><erp>0.2</erp><contact_max_correcting_vel>100</contact_max_correcting_vel><contact_surface_layer>0.001</contact_surface_layer></constraints></ode><max_step_size>0.004</max_step_size><real_time_factor>1</real_time_factor><real_time_update_rate>250</real_time_update_rate><magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field></physics>

注意事项：

ROS程序订阅的的消息为/mavros/imu/data，而非/mavros/imu/data_raw。
ROS程序发布的给定姿态topic为/mavros/setpoint_raw/attitude，而非/mavros/setpoint_attitude/attitude。
ROS发布的速度指令topic为/mavros/setpoint_velocity/cmd_vel_unstamped
姿态解算部分参考了Ardupilot中的函数。

效果图为：

ROS程序代码为：

/*** @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 <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/Thrust.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <sensor_msgs/Imu.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){current_state = *msg;
}struct attitude_quat{float q1;float q2;float q3;float q4;
};float _roll,_pitch,_yaw;
struct attitude_quat att_quat;
sensor_msgs::Imu current_imudata;
Eigen::Quaternionf current_imu_quat;float get_euler_roll(float q1,float q2,float q3,float q4)
{return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
}// get euler pitch angle
float get_euler_pitch(float q1,float q2,float q3,float q4)
{return asin(2.0f*(q1*q3 - q4*q2));
}// get euler yaw angle
float get_euler_yaw(float q1,float q2,float q3,float q4)
{return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
}// create eulers from a quaternion
void to_euler(float &roll, float &pitch, float &yaw)
{roll = get_euler_roll(current_imudata.orientation.w,current_imudata.orientation.x,current_imudata.orientation.y,current_imudata.orientation.z);pitch = get_euler_pitch(current_imudata.orientation.w,current_imudata.orientation.x,current_imudata.orientation.y,current_imudata.orientation.z);yaw = get_euler_yaw(current_imudata.orientation.w,current_imudata.orientation.x,current_imudata.orientation.y,current_imudata.orientation.z);
}void imu_cb(const sensor_msgs::Imu::ConstPtr& msg){current_imudata = *msg;current_imu_quat.w() = current_imudata.orientation.w;current_imu_quat.x() = current_imudata.orientation.x;current_imu_quat.y() = current_imudata.orientation.y;current_imu_quat.z() = current_imudata.orientation.z;to_euler(_roll,_pitch,_yaw);// ROS_INFO("PASS THE IMU DATA %f",current_imudata.orientation.w);
}void from_euler(float roll, float pitch, float yaw)
{const float cr2 = cosf(roll*0.5f);const float cp2 = cosf(pitch*0.5f);const float cy2 = cosf(yaw*0.5f);const float sr2 = sinf(roll*0.5f);const float sp2 = sinf(pitch*0.5f);const float sy2 = sinf(yaw*0.5f);att_quat.q1 = cr2*cp2*cy2 + sr2*sp2*sy2;att_quat.q2 = sr2*cp2*cy2 - cr2*sp2*sy2;att_quat.q3 = cr2*sp2*cy2 + sr2*cp2*sy2;att_quat.q4 = cr2*cp2*sy2 - sr2*sp2*cy2;
}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_attitude_pub = nh.advertise<mavros_msgs::AttitudeTarget>("mavros/setpoint_raw/attitude",10);ros::Subscriber imu_attitude_sub = nh.subscribe<sensor_msgs::Imu>("mavros/imu/data",10,imu_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 2Hzros::Rate rate(20.0);// wait for FCU connectionwhile(ros::ok() && !current_state.connected){ros::spinOnce();rate.sleep();}geometry_msgs::PoseStamped pose;pose.pose.position.x = 1;pose.pose.position.y = 2;pose.pose.position.z = 5;float roll_deg = 12;float pitch_deg = 15;float yaw_deg = 0;from_euler(roll_deg*M_PI/180, pitch_deg*M_PI/180, yaw_deg*M_PI/180);mavros_msgs::AttitudeTarget attitude_raw;attitude_raw.orientation.w = att_quat.q1;attitude_raw.orientation.x = att_quat.q2;attitude_raw.orientation.y = att_quat.q3;attitude_raw.orientation.z = att_quat.q4;attitude_raw.thrust = 0.8;attitude_raw.type_mask = 0b00000111;pose.pose.orientation.w = att_quat.q1;pose.pose.orientation.x = att_quat.q2;pose.pose.orientation.y = att_quat.q3;pose.pose.orientation.z = att_quat.q4;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();int16_t step_counter = 0;int8_t step_one = 1;Eigen::Matrix3f _rotMatrix;Eigen::Vector3f eular_angle;while(ros::ok()){_rotMatrix = current_imu_quat.toRotationMatrix();eular_angle = _rotMatrix.eulerAngles(2,1,0);if(!(step_counter%20))ROS_INFO("Imu_data = %f %f %f", _roll*180/M_PI,_pitch*180/M_PI,_yaw*180/M_PI);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();ROS_INFO("try to arm");}else{if(step_counter<400){step_one = 1;}     else{step_one = 0;}}}if(step_one){local_pos_pub.publish(pose);}   else{local_attitude_pub.publish(attitude_raw);}ros::spinOnce();rate.sleep();step_counter++;}return 0;
}

ROS_PX4_gazebo学习记录相关推荐

Pytorch学习记录-torchtext和Pytorch的实例（使用神经网络训练Seq2Seq代码）
Pytorch学习记录-torchtext和Pytorch的实例1 0. PyTorch Seq2Seq项目介绍 1. 使用神经网络训练Seq2Seq 1.1 简介,对论文中公式的解读 1.2 数据预 ...
HTML5与CSS3权威指南之CSS3学习记录
title: HTML5与CSS3权威指南之CSS3学习记录 toc: true date: 2018-10-14 00:06:09 学习资料--<HTML5与CSS3权威指南>(第3版) ...
springboot @cacheable不起作用_Springboot学习记录13 使用缓存：整合redis
本学习记录的代码,部分参考自gitee码云的如下工程.这个工程有详尽的Spingboot1.x教程.鸣谢! https://gitee.com/didispace/SpringBoot-Learnin ...
【Cmake】Cmake学习记录
Cmake学习记录 1.1 常例 add_library(gen_reference_infogen_reference_info/gen_reference_info.hgen_reference_ ...
ASP.NETCore学习记录(一)
ASP.NETCore学习记录(一) asp.net core介绍 Startup.cs ConfigureServices Configure 0. ASP.NETCore 介绍 ASP.N ...
Android开发技术周报176学习记录
Android开发技术周报176学习记录教程当 OkHttp 遇上 Http 2.0 http://fucknmb.com/2018/04/16/%E5%BD%93OkHttp%E9%81%87% ...
add函数 pytorch_Pytorch学习记录-Pytorch可视化使用tensorboardX
Pytorch学习记录-Pytorch可视化使用tensorboardX 在很早很早以前(至少一个半月),我做过几节关于tensorboard的学习记录. https://www.jianshu.co ...
java之字符串学习记录
java之字符串学习记录 public class StringDemo { public static void main(String[] args) { //静态初始化字符串 String s1 ...
Redis的学习记录
Redis的学习记录 1.先导了解 1.1 NOSQL概述 1.1.1 为什么要用NoSql? 1.1.2 NoSql了解 1.1.3 NoSql特点 1.1.4 NoSQL的四大分类 2. Redi ...

ROS_PX4_gazebo学习记录

在官方程序上（PX4 wiki上为offboard起飞到2m高度）进行更改，实现首先起飞到固定点（x=1,y=2,z=5), 然后按照给定角度飞行。

ROS_PX4_gazebo学习记录相关推荐

最新文章

热门文章

ROS_PX4_gazebo学习记录

在官方程序上（PX4 wiki上为offboard起飞到2m高度） 进行更改，实现首先起飞到固定点（x=1,y=2,z=5), 然后按照给定角度飞行。

ROS_PX4_gazebo学习记录相关推荐

最新文章

热门文章

在官方程序上（PX4 wiki上为offboard起飞到2m高度）进行更改，实现首先起飞到固定点（x=1,y=2,z=5), 然后按照给定角度飞行。