【ROS-Stage】Stage机器人仿真实验（一）

环境：64位 ubuntu16.04，代码见：https://github.com/cr19941016/ROS_stage_test

1. 安装ROS
   更换软件源（设置 --> Software & Update），会下载的更快。
   stage_ros仿真包是ros自带的，安装完之后就有了。
2. 按照教程，创建一个工作空间
   记得添加 source：

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc 

3. 按照教程，创建ROS程序包
4. 下载 turtlebot_simulator，将里面的 /turtlebot_stage/maps/ 复制出来，放在新建的程序包文件夹中（beginner_tutorials）。
5. 共享内存用于ROS节点和非ROS进程之间的通信。
   创建共享内存区的数据结构，存放在 sharemem.h中。

#ifndef SHAREMEM_H
#define SHAREMEM_H
struct ShareMem
{
  double x; //  x方向的速度(前后)
  double y; //  y方向的速度(左右)
  double z; //  旋转
};
#endif

6. 新建talker节点。
   创建共享内存、监听共享内存区域的速度变化和发布速度话题。

/****************************************
*文件名: talker.cpp            *
*简介: 链接非ROS进程和ROS节点的ROS节点   *
*BY: CR (qq: 414481619)         *
*创建时间: 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的定时器

  
  ShareMem _ShareMem;
  // 创建共享内存 ，相当于打开文件，文件不存在则创建
  int shmid = shmget((key_t)2333, sizeof(_ShareMem), 0666|IPC_CREAT);  
  if(shmid==-1)
  {
    perror("shmget err");
    return errno;
  }
  cout<<shmid<<endl; 

  // 将共享内存段连接到进程地址空间, 第二个参数shmaddr为NULL，核心自动选择一个地址
  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);  //将共享内存段与当前进程脱离
  shmctl(shmid, IPC_RMID, NULL); //IPC_RMID为删除内存段
  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 ~/catkin_ws
catkin_make

7. 创建launch文件
   新建launch文件夹，创建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>  

8. 创建速度控制父类
   便于其他速度控制类的编写，其他速度控制类只需要继承父类的框架，完成自己的mainloop()就好。

/****************************************
*文件名: VelControl.h          *
*简介: 速度控制父类         *
*BY: CR (qq: 414481619)         *
*创建时间: 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);

    // 绑定共享内存
    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);  //将共享内存段与当前进程脱离
    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);
}

9. 这是一个键盘控制速度的例程（非常粗糙，仅供参考）

/****************************************
*文件名: keyboard_not_ros.cpp      *
*简介: 速度控制例程         *
*BY: CR (qq: 414481619)         *
*创建时间: 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;
}

10. 编译程序
    在 ~/catkin_ws/src/beginner_tutorials/src/ 目录下，终端输入：

g++ -o keyboard_not_ros keyboard_not_ros.cpp 

11. 运行程序
    终端输入，启动ROS stage和其他相关节点：

roslaunch beginner_tutorials stage_test.launch 

另一个终端输入：

./keyboard_not_ros

12. 结果输出

    
    最后的运行结果

备注：但是stage不通过ROS也能使用，具体看下一篇文章。