參考http://www.reibang.com/p/682e80efbe5a
首先創(chuàng)建工作空間,創(chuàng)建工作空間
mkdir -p ~/test_ws/src
cd ~/test_ws
catkin_make
source devel/setup.bash
在～t/test_ws/src下創(chuàng)建功能包，創(chuàng)建功能包
catkin_create_pkg beginner_tutorials std_msgs roscpp rospy
在～/test_ws/src/beginner_tutorials中拷貝從/opt/ros/kinetic/share/turtlebot_stage中復(fù)制的maps文件夾
在～/test_ws/src/beginner_tutorials/src中創(chuàng)建sharemem.h文件驹止，其中定義共享空間中的數(shù)據(jù)結(jié)構(gòu)淋样。文件內(nèi)容為
#ifndef SHAREMEM_H
#define SHAREMEM_H
struct ShareMem
{
double x; // x方向的速度(前后)
double y; // y方向的速度(左右)
double z; // 旋轉(zhuǎn)
};
#endif
新建talker節(jié)點(diǎn)。創(chuàng)建共享內(nèi)存禀综、監(jiān)聽共享內(nèi)存區(qū)域的速度變化和發(fā)布速度話題截驮。
/****************************************
*文件名: talker.cpp *
*簡(jiǎn)介: 鏈接非ROS進(jìn)程和ROS節(jié)點(diǎn)的ROS節(jié)點(diǎn) *
*BY: CR (qq: 414481619) *
*創(chuàng)建時(shí)間: 2018.10.09 *
****************************************/
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <stdio.h>
#include <sys/shm.h>
#include <errno.h>
#include <sys/types.h>
#include <stdlib.h>
#include <pthread.h>
#include "sharemem.h"
using namespace std;
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
ros::Rate loop_rate(100); // 100Hz的定時(shí)器
ShareMem _ShareMem;
// 創(chuàng)建共享內(nèi)存 捌浩，相當(dāng)于打開文件，文件不存在則創(chuàng)建
int shmid = shmget((key_t)2333, sizeof(_ShareMem), 0666|IPC_CREAT);
if(shmid==-1)
{
perror("shmget err");
return errno;
}
cout<<shmid<<endl;
// 將共享內(nèi)存段連接到進(jìn)程地址空間, 第二個(gè)參數(shù)shmaddr為NULL笛臣，核心自動(dòng)選擇一個(gè)地址
ShareMem *shareMem = (ShareMem *)shmat(shmid, NULL, 0);
if (shareMem == (void *)-1 )
{
perror("shmat err");
return errno;
}
shareMem->x = 0;
shareMem->y = 0;
shareMem->z = 0;
pthread_mutex_t mutex;
pthread_mutex_init(&mutex, NULL);
while (ros::ok())
{
geometry_msgs::Twist cmdvel_msg;
pthread_mutex_lock(&mutex);
cmdvel_msg.linear.x = shareMem->x;
cmdvel_msg.linear.y = shareMem->y;
cmdvel_msg.angular.z = shareMem->z;
pthread_mutex_unlock(&mutex);
chatter_pub.publish(cmdvel_msg);
ros::spinOnce();
loop_rate.sleep();
}
pthread_mutex_destroy(&mutex);
shmdt(shareMem); //將共享內(nèi)存段與當(dāng)前進(jìn)程脫離
shmctl(shmid, IPC_RMID, NULL); //IPC_RMID為刪除內(nèi)存段
return 0;
}
在beginner_tutorials 的 CMakeLists.txt 中添加：
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
在終端中：
cd ~/test_ws
catkin_make
創(chuàng)建launch文件新建launch文件夾云稚，創(chuàng)建stage_tast.launch文件。
<!--
stage_test
- stage_ros
- talker
-->
<launch>
<arg name="world_file" default=" $(find beginner_tutorials)/maps/stage/maze.world"/>
<node pkg="stage_ros" type="stageros" name="stageros" args="$(arg world_file)">
<param name="base_watchdog_timeout" value="0.5"/>
<remap from="odom" to="odom"/>
<remap from="base_pose_ground_truth" to="base_pose_ground_truth"/>
<remap from="cmd_vel" to="cmd_vel"/>
</node>
<node pkg="beginner_tutorials" type="talker" name="talker">
</node>
</launch>
創(chuàng)建速度控制父類沈堡，便于其他速度控制類的編寫静陈，其他速度控制類只需要繼承父類的框架，完成自己的mainloop()就好诞丽。
/****************************************
*文件名: VelControl.h *
*簡(jiǎn)介: 速度控制父類 *
*BY: CR (qq: 414481619) *
*創(chuàng)建時(shí)間: 2018.10.09 *
****************************************/
// this code ism't in ros system
#include <iostream>
#include <stdio.h>
#include <sys/shm.h>
#include <errno.h>
#include <sys/types.h>
#include <stdlib.h>
#include <pthread.h>
#include <unistd.h>
#include "sharemem.h"
using namespace std;
class VelControl
{
public:
VelControl();
~VelControl();
void mainloop();
protected:
ShareMem *shareMem;
pthread_mutex_t mutex;
void writeVel(double x, double y, double z);
};
VelControl::VelControl()
{
pthread_mutex_init(&mutex, NULL);
// 綁定共享內(nèi)存
int shmid = shmget((key_t)2333, 0, 0);
if(shmid != -1)
{
shareMem =(ShareMem *)shmat(shmid, NULL, 0);
cout<<shmid<<endl;
}
else
{
cout<<"ERROR: stage_test haven't started"<<endl;
exit(1);
}
}
VelControl::~VelControl()
{
shmdt(shareMem); //將共享內(nèi)存段與當(dāng)前進(jìn)程脫離
pthread_mutex_destroy(&mutex);
}
void VelControl::writeVel(double x, double y, double z)
{
pthread_mutex_lock(&mutex);
shareMem->x = x;
shareMem->y = y;
shareMem->z = z;
pthread_mutex_unlock(&mutex);
}
鍵盤控制例程
/****************************************
*文件名: keyboard_not_ros.cpp *
*簡(jiǎn)介: 速度控制例程 *
*BY: CR (qq: 414481619) *
*創(chuàng)建時(shí)間: 2018.10.09 *
****************************************/
// this code isn't in ros system
#include "VelControl.h"
using namespace std;
class keyboard_not_ros:VelControl
{
public:
void mainloop();
};
void keyboard_not_ros::mainloop()
{
char c = 'a';
while(c!='q')
{
cout<<"please input a letter"<<endl;
c = getchar();
getchar();
double x,y,z;
switch(c)
{
case 'W':
case 'w':
x = 1;
y = 0;
z = 0;
break;
case 'S':
case 's':
x = -1;
y = 0;
z = 0;
break;
case 'A':
case 'a':
x = 0;
y = 0;
z = 1;
break;
case 'D':
case 'd':
x = 0;
y = 0;
z = -1;
break;
default:;
}
writeVel(x, y, z);
usleep(100*1000);
writeVel(0,0,0);
}
}
int main(int argc, char const *argv[])
{
keyboard_not_ros k;
k.mainloop();
return 0;
}
編譯程序鲸拥，在 ~/test_ws/src/beginner_tutorials/src/ 目錄下，終端輸入：
g++ -o keyboard_not_ros keyboard_not_ros.cpp
啟動(dòng)launch文件
roslaunch beginner_tutorials stage_test.launch
另一個(gè)終端中僧免，～/test_ws/src/beginner_tutorials/src下輸入
./keyboard_not_ros
