测试运行
roslaunch hector_quadrotor_demo gimbal_empty_world_test_flight_gazebo.launch
运行之后得到下图
查看各界点关系
ros rqt_graph rqt_graph
ROS节点编写(复习)
参考链接
创建一个包
catkin_create_pkg simple std_msgs roscpp rospy
编译
cd ..
catkin_make
创建.cpp
touch simple.cpp
chmod 777 simple.cpp
编写一个发布话题
vim simple.cpp
将如下代码加入：
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
//ros::init(argc,"node_name",ros::init_options::AnonymousName)
//argc,argv 传递命令行的参数，ROS会解析这些参数自动
//修改他们，使你自己在调用ros::init时无需更改
//ros::init_options，三个选项：
//NoSigintHandler：如果不使用ROS自己的中断程序，想
//要自己设置退出方式设置这一选项，详解例1
//AnonymousName：添加随机数到节点名后边防止重名
//NoRosout：不將ROS控制台的结果输出到/rosout节点
ros::NodeHandle n;
//定义命名空间 n
//ros在创建命名空间时回自动调用ros::start，并在最后
//一个调用ros::NodeHandle时自动调用ros::shutdown
//想要手动调用ros::satrt管理周期，但需要使用
//ros::shutdown关闭
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
//ros::Publisher 发布一个 话题
//NodeHandle::advertise<消息类型>(“话题名”，缓冲区大小)
ros::Rate loop_rate(10);
//定义一个循环周期10HZ
int count = 0;
while (ros::ok())
//ros::ok检测ros是否正在执行
{
std_msgs::String msg;//定义std_msgs::String变量,详细格式通过rosmsg show std_msgs/String 查看
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
//打印信息到终端
hatter_pub.publish(msg);
//发布消息
ros::spinOnce();
//回调函数，在函数之后接受订阅的消息
//ros::spin()只执行一次
loop_rate.sleep();
//调用ros::Rate
++count;
}
return 0;
}
例1：自定义中断标志
#include <ros/ros.h>
#include <signal.h>
void mySigintHandler(int sig)
{
//所有中断最后必须调用关闭程序
ros::shutdown();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_node_name", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
signal(SIGINT, mySigintHandler);
//自定义中断，必须在ros::NodeHandle之后，第一个创建
ros::spin();
return 0;
}
编写一个订阅话题
#include "ros/ros.h"
#include "std_msgs/String.h"
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
//订阅subscribe(话题，缓存队列，回调函数)
ros::spin();
//
return 0;
}
修改CMakeLists.txt,添加
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
编写一个服务
自定义服务类型和消息类型参考
在simple/srv中定义一个消息类型如下
int64 a
int64 b
---
int64 sum
定义一个服务端
#include"ros/ros.h"
#include"simple/AddtwoInts.h"
bool add(simple::AddtwoInts::Request &req,
simple::AddtwoInts::Response &res)
{
res.sum = req.a + req.b;
ROS_INFO("request:x=%ld,y=%ld",(long int)req.a,(long int)req.b);
ROS_INFO("sending back response:[%ld]",(long int)res.sum);
return true;
}
int main(int argc,char **argv)
{
ros::init(argc, argv, "add_two_ints_server");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("add_two_ints",add);
//创建一个服务
ROS_INFO("Ready to add two ints.");
ros::spin();
return 0;
}
定义一个客户端
#include<cstdlib>
#include"ros/ros.h"
#include"simple/AddtwoInts.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_two_ints_client");
if (argc !=3)
{
ROS_INFO("usage:add_two_ints_client X Y");
return 1;
}
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<simple::AddtwoInts>("add_two_ints");
simple::AddtwoInts srv;
srv.request.a=atoll(argv[1]);
srv.request.b=atoll(argv[2]);
if (client.call(srv))
{
ROS_INFO("Sum: %ld",(long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service add_two_ints");
return 1;
}
return 0;
}
修改CMakeLists.txt：
add_service_files(
FILES
AddtwoInts.srv
)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
)
catkin_package(
LIBRARIES service_demo
CATKIN_DEPENDS message_runtime
)
add_executable(
#listener src/simple/listener.cpp
server src/simple/server.cpp
)
target_link_libraries(
#listener ${catkin_LIBRARIES}
server ${catkin_LIBRARIES}
)
add_executable(
#listener src/simple/listener.cpp
client src/simple/client.cpp
)
target_link_libraries(
#listener ${catkin_LIBRARIES}
client ${catkin_LIBRARIES}
)
修改package.xml：
<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend>
<build_depend>message_runtime</build_depend>
<exec_depend>message_runtime</exec_depend>
测试：略
代码阅读`
QuadrotorControl自定义类：
该类包含功能如下：
void SUB_UAV_StateInfo_Callback
//订阅无人机信息
bool UAV_PositionControl
//无人机位置控制模式
bool UAV_HeadingControl
//控制无人机的航向角
void UAV_VelocityControl
//无人机速度控制
void Gimbal_PitchPositionControl
//无人机俯仰角位置控制
void Gimbal_Pitch_SelfStable
//云台俯仰自稳控制
void Get_YeJian_Position_From_YePian
// 由叶片中心点的位置 计算叶尖的位置
void Get_WayPoint_From_YeJianPos
//由叶尖位置计算各航点
void Visualize_UAV_MotionTrajectory
//无人机运动轨迹可视化
void UAV_FlightControl_ToTargetPosition_ThreeDimensional
// 无人机三维空间飞行控制
bool Fly_To_Point_CrossTrack_XY（）
//飞向目标点
bool Uav_Hover_Control（）
//无人机悬停控制
ImageConverter 类：
该类提供了Ros图像与Opencv图像的相互转换（C++方式）
ROS的图像数据格式比OPENCV数据格式更复杂。
CvBridge提供了两种方式实现双方的类型转换。
参考链接
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source, const std::string& encoding = std::string());
CvImagePtr toCvCopy(const sensor_msgs::Image& source, const std::string& encoding = std::string());
//在修改数据的地方，复制一份ros的信息数据
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source, const std::string& encoding = std::string());
CvImageConstPtr toCvShare(const sensor_msgs::Image&source, const boost::shared_ptr<void const>& tracked_object,const std::string& encoding = std::string());
//分享由ros消息所拥有的数据，而不用复制
使用方式
添加image_transport、cv_bridge、opencv包含进你的 package.xml中
<span style="font-size:18px;">#include<cv_bridge/cv_bridge.h>#include <sensor_msgs/image_encodings.h></span>
<span style="font-size:18px;">#include<opencv2/imgproc/imgproc.hpp>#include <opencv2/highgui/highgui.hpp></span>
