【嵌入式】ROS通信编程

DEVIE352

于 2024-03-16 13:28:22 发布

阅读量1.4k

点赞数 23

文章标签：嵌入式硬件 c++

本文链接：https://blog.csdn.net/weixin_62901798/article/details/136740561

版权

一、ROS动作编程: 客户端发送一个运动目标，模拟机器人运动到目标位置的过程

1.创建工作区间

在这里插入图片描述

2.定义action文件

在learn_action文件下创建action文件，在action文件下创建TurtleMove.action文件，并在TurtleMove.action文件内输入代码:

# Define the goal 
float64 turtle_target_x  
# Specify Turtle's　target position 
float64 turtle_target_y 
float64 turtle_target_theta 
---
# Define the result 
float64 turtle_final_x 
float64 turtle_final_y 
float64 turtle_final_theta 
--- 
# Define a feedback message 
float64 present_turtle_x 
float64 present_turtle_y 
float64 present_turtle_theta

在这里插入图片描述

3.创建cpp文件

在learn_action的src文件夹下，创建TurtleMove_server.cpp文件和TurtleMove_client.cpp文件

//TurtleMove_server.cpp
 /*  
　　　此程序通过通过动作编程实现由client发布一个目标位置
　　　然后控制Turtle运动到目标位置的过程
 */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionServer<learn_action::TurtleMoveAction> Server;

struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_original_pose,turtle_target_pose;

ros::Publisher turtle_vel;

void posecallback(const turtlesim::PoseConstPtr& msg)
{
  ROS_INFO("Turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);
  turtle_original_pose.x=msg->x;
  turtle_original_pose.y=msg->y;
  turtle_original_pose.theta=msg->theta;
 }

// 收到action的goal后调用该回调函数
void execute(const learn_action::TurtleMoveGoalConstPtr& goal, Server* as)
{
    learn_action::TurtleMoveFeedback feedback;
     ROS_INFO("TurtleMove is working.");
    turtle_target_pose.x=goal->turtle_target_x;
    turtle_target_pose.y=goal->turtle_target_y;
    turtle_target_pose.theta=goal->turtle_target_theta;

    geometry_msgs::Twist vel_msgs;
    float break_flag;

    while(1)
    {
        ros::Rate r(10);

        vel_msgs.angular.z = 4.0 * (atan2(turtle_target_pose.y-turtle_original_pose.y,
                                   turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);
        vel_msgs.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                      pow(turtle_target_pose.y-turtle_original_pose.y, 2));
        break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                        pow(turtle_target_pose.y-turtle_original_pose.y, 2));
        turtle_vel.publish(vel_msgs);

        feedback.present_turtle_x=turtle_original_pose.x;
        feedback.present_turtle_y=turtle_original_pose.y;
        feedback.present_turtle_theta=turtle_original_pose.theta;
        as->publishFeedback(feedback);
        ROS_INFO("break_flag=%f",break_flag);
        if(break_flag<0.1) break;
        r.sleep();
    }
        // 当action完成后，向客户端返回结果
        ROS_INFO("TurtleMove is finished.");
        as->setSucceeded();
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "TurtleMove_server");
    ros::NodeHandle n,turtle_node;
    ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&posecallback); //订阅小乌龟的位置信息
    turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度
    // 定义一个服务器
        Server server(n, "TurtleMove", boost::bind(&execute, _1, &server), false);
        // 服务器开始运行
        server.start();
        ROS_INFO("server has started.");
    ros::spin();

    return 0;
}

//TurtleMove_client.cpp
#include <actionlib/client/simple_action_client.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionClient<learn_action::TurtleMoveAction> Client;

struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_present_pose;

// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const learn_action::TurtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The TurtleMove is finished!");
    ros::shutdown();
}

// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}

// 收到feedback后调用该回调函数
void feedbackCb(const learn_action::TurtleMoveFeedbackConstPtr& feedback)
{
    ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,
                   feedback->present_turtle_y,feedback->present_turtle_theta);
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "TurtleMove_client");

    // 定义一个客户端
    Client client("TurtleMove", true);

    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    // 创建一个action的goal
    learn_action::TurtleMoveGoal goal;
    goal.turtle_target_x = 1;
    goal.turtle_target_y = 1;
    goal.turtle_target_theta = 0;

    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    ros::spin();

    return 0;
}

在这里插入图片描述

4.修改文件

修改package.xml文件

在这里插入图片描述
修改CmakeLists.txt文件

在这里插入图片描述
并在文件末尾添加

add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${catkin_LIBRARIES})
add_dependencies(TurtleMove_client ${PROJECT_NAME}_gencpp)
 
add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${catkin_LIBRARIES})
add_dependencies(TurtleMove_server ${PROJECT_NAME}_gencpp)

5.编译程序

cd ~/catkin_ws
catkin_make

在这里插入图片描述

6.运行程序

设置环境变量，启动ros核心

source ~/catkin_ws/devel/setup.bash
roscore

新建终端，运行小海龟

source ~/catkin_ws/devel/setup.bash
rosrun turtlesim turtlesim_node

在这里插入图片描述

新建终端，运行TurtleMove_server.cpp

source ./devel/setup.bash
rosrun learn_action TurtleMove_server

新建终端，运行TurtleMove_client.cpp

source ./devel/setup.bash
rosrun learn_action TurtleMove_client

在这里插入图片描述

二、在Ubuntu系统下安装图像处理开源软件库 Opencv并练习编译

（一）、安装OpenCV

1.下载安装包：

https://codeload.github.com/opencv/opencv/zip/refs/tags/3.4.11

2.解压缩

下载好后，可以在文件夹中看见被下载好的压缩包文件，在终端打开该路径，使用指令解压：

unzip opencv-3.4.11.zip

进入解压后的文件夹：opencv-3.4.11

cd opencv-3.4.11

进入 root 用户，并更新一下。

sudo su
sudo apt-get update

在这里插入图片描述

3.下载cmake并完成配置

sudo apt-get  install cmake

在这里插入图片描述
安装依赖库

sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev libavformat-dev libjpeg.dev libtiff5.dev libswscale-dev libjasper-dev

创建文件build,并进入

mkdir build
cd build

使用 cmake 编译参数，或者使用第二条默认参数，都可以的。

cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
cmake ..

4.make编译

在build文件夹下

sudo make
sudo make -j4//如果你想更快编译完，可以使用命令：sudo make -j4 ，而 -j4 表示使用 4 个线程进行编译。

在这里插入图片描述
安装

sudo make install

5.配置环境

修改 opencv.conf 文件，打开后的文件是空的，添加 opencv 库的安装路径：/usr/local/lib
更新系统共享链接库

sudo ldconfig

配置 bash ，修改 bash.bashrc 文件

sudo gedit /etc/bash.bashrc

在文件末尾加入

PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig
export PKG_CONFIG_PATH

保存退出，然后执行如下命令使得配置生效:

source /etc/bash.bashrc

更新

sudo updatedb
//确保安装了mlocate
命令：apt-get install mlocate

接下来查看 opencv 的版本信息。

pkg-config --modversion opencv

在这里插入图片描述
以上即配置成功。

（二）、展示图片

1.创建文件

创建文件夹code

mkdir code
cd code

在code文件夹中创建test1.cpp

gedit test1.cpp

写入下列代码

#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
int main(int argc, char** argv)
{
	CvPoint center;
    double scale = -3; 

	IplImage* image = cvLoadImage("lena.jpg");
	argc == 2? cvLoadImage(argv[1]) : 0;
	
	cvShowImage("Image", image);
	
	
	if (!image) return -1; 	center = cvPoint(image->width / 2, image->height / 2);
	for (int i = 0;i<image->height;i++)
		for (int j = 0;j<image->width;j++) {
			double dx = (double)(j - center.x) / center.x;
			double dy = (double)(i - center.y) / center.y;
			double weight = exp((dx*dx + dy*dy)*scale);
			uchar* ptr = &CV_IMAGE_ELEM(image, uchar, i, j * 3);
			ptr[0] = cvRound(ptr[0] * weight);
			ptr[1] = cvRound(ptr[1] * weight);
			ptr[2] = cvRound(ptr[2] * weight);
		}

	Mat src;Mat dst;
	src = cvarrToMat(image);
	cv::imwrite("test.png", src);

    cvNamedWindow("test",1);  	imshow("test", src);
	 cvWaitKey();
	 return 0;
}

2.编译执行

g++ test1.cpp -o test1 `pkg-config --cflags --libs opencv`
//使用 pkg-config 时，选项 --cflags 它是用来指定程序在编译时所需要头文件所在的目录,选项 --libs 则是指定程序在链接时所需要的动态链接库的目录

在code中放入图片命名为lena.jpg
在这里插入图片描述

3.运行

./test1

在这里插入图片描述

（三）、使用opencv库编写打开摄像头压缩视频的程序

1.虚拟机获取摄像头权限

●使用快捷键 Win + R ，输入 services.msc ，并回车
●找到 VMware USB Arbitration S… 服务，确保启动了。
在这里插入图片描述
●点击 “ 虚拟机 ” ，然后点击 “ 设置(S)… ”。
选择 “ USB控制器 ” ，将 “ USB兼容性 ” 设置为 “ USB 3.1 ” ，并点击确定。
●选择 “ 虚拟机 ” ，再选择 “ 可移动设备 ” ，再选择 “ Quanta USB2.0 VGA UVC WebCam ” ，最后点击 “ 连接 ” ，再弹出的窗口内点击 “ 确定 ” 。
● 虚拟机右下角这个摄像头图标有个小绿点，则连接成功。
在这里插入图片描述

2.播放视频

创建test2.cpp

gedit test2.cpp

写入代码

#include <opencv2/opencv.hpp>
using namespace cv;
int main()
{
	//从摄像头读取视频
	VideoCapture capture("man.mp4");
	//循环显示每一帧
	while(1){
		Mat frame;//定义一个Mat变量，用于存储每一帧的图像
		capture >> frame;//读取当前帧
		if(frame.empty())//播放完毕，退出
			break;
		imshow("读取视频帧",frame);//显示当前帧
		waitKey(30);//掩饰30ms
	}
	system("pause");
	return 0;
}

在文件夹中放入视频编译运行

g++ test2.cpp -o test2 `pkg-config --cflags --libs opencv`
./test2

3.录制视频

创建test3.cpp

gedit test3.cpp

写入代码

/*********************************************************************
打开电脑摄像头，空格控制视频录制，ESC退出并保存视频RecordVideo.avi
*********************************************************************/
#include<iostream>
#include <opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
using namespace cv;
using namespace std;

int main()
{
	//打开电脑摄像头
	VideoCapture cap(0);
	if (!cap.isOpened())
	{
		cout << "error" << endl;
		waitKey(0);
		return 0;
	}

	//获得cap的分辨率
	int w = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_WIDTH));
	int h = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_HEIGHT));
	Size videoSize(w, h);
	VideoWriter writer("RecordVideo.avi", CV_FOURCC('M', 'J', 'P', 'G'), 25, videoSize);
Mat frame;
	int key;//记录键盘按键
	char startOrStop = 1;//0  开始录制视频； 1 结束录制视频
	char flag = 0;//正在录制标志 0-不在录制； 1-正在录制

	while (1)
	{
		cap >> frame;
		key = waitKey(100);
		if (key == 32)//按下空格开始录制、暂停录制   可以来回切换
		{
			startOrStop = 1 - startOrStop;
			if (startOrStop == 0)
			{
				flag = 1;
			}
		}
		if (key == 27)//按下ESC退出整个程序，保存视频文件到磁盘
		{
			break;
		}

		if (startOrStop == 0 && flag==1)
		{
			writer << frame;
			cout << "recording" << endl;
		}
		else if (startOrStop == 1)
		{
			flag = 0;
			cout << "end recording" << endl;
			
		}
		imshow("picture", frame);
	}
	cap.release();
	writer.release();
	destroyAllWindows();
	return 0;
}

编译运行

g++ test3.cpp -o test3 `pkg-config --cflags --libs opencv`
./test3

在这里插入图片描述
空格开始录制，esc键录制结束。
生成了一个.avi的文件，就是录制的视频

如果要求打开你硬盘上一个视频文件来播放，请问示例代码1第7行代码如何修改？

#include <opencv2/opencv.hpp>
using namespace cv;
int main()
{
	//从摄像头读取视频
	VideoCapture capture("man.mp4");//VideoCapture capture(0)；打开man.mp4视频文件
	//循环显示每一帧
	while(1){
		Mat frame;//定义一个Mat变量，用于存储每一帧的图像
		capture >> frame;//读取当前帧
		if(frame.empty())//播放完毕，退出
			break;
		imshow("读取视频帧",frame);//显示当前帧
		waitKey(30);//掩饰30ms
	}
	system("pause");
	return 0;
}

在示例代码1第9行的while循环中，Mat是一个什么数据结构？为什么一定要加一句waitKey延时代码，删除它行不行？
while循环体中的 Mat 数据结构其实是一个点阵，对应图像上的每一个点，点的集合形成了一帧图像。语句：waitKey(30) ，中的参数● 单位是 ms 毫秒，也就是每一帧间隔 30ms，该语句时不能删除的，否则会执行错误，无法播放视频或录制视频。
示例代码1代码会在while循环中一直运行，你如果试图用鼠标关闭图像显示窗口，会发现始终关不掉。需要用键盘Ctrl+C 强制中断程序，非常不友好。
因此进行改进代码如下：

#include <opencv2/opencv.hpp>
using namespace cv;
int main()
{
VideoCapture capture(0);
while(1)
{
Mat frame;
capture >> frame;
imshow("读取视频帧",frame);
if(frme.empt())//
{
break;
}//
waitKey(30);
}
system("pause");
return 0;
}