树莓派搭建ROS及简单应用

1，目的

  使用树莓派GPIO控制电机驱动，实现小车的前进后退左转右转停止运行。

   深入理解ROS框架

   使用wiringPi控制pi的GPIO

2，环境搭建

  a，小车部分：4个小电机，小车底盘和电机驱动（L298N）

   b，pi3，搭载ubuntu－mate嵌入式系统，ROS core，wiringPi，通过PC ssh 客户端访问

   c，PC，virtualBox虚拟机中使用ubuntu－mate桌面系统，完整ROS

   d，pi3和PC虚拟机（网络使用桥接）连到同一个路由器下，可以网络通讯

  ubuntu－mate系统，ROS框架，wiringPi安装办法请在相应官网查询

3，实现

   在ROS教程中beginning tutorial 建立两个node实现，pi上node subscribe 一个commands的topic，获取消息，控制小车运行。为了实现简单，msg使用UInt8，PC虚拟机上修改turtle_teleop_key.py，实现通过键盘向commands 的topic发送消息 也可以通过topic pub 方式实现控制。

   困难1：小车控制部分代码使用C++语言完成，要调用C的wiringPi库，并且实现通过catkin_make编译。好在已经有人解决了。

                 c++ - Add wiringPi lib to cmake on RaspberryPi - Stack Overflow

    困难2:  PC虚拟机上运行roscore，在虚拟机上和pi上启动node通信总是出现异常，不能识别域名。解决办法参考

                 http://wiki.ros.org/ROS/NetworkSetupSetting a name explicitly 

export ROS_HOSTNAME=10.0.0.1

4，代码

    a，小车封装类，通过wiringPi控制，实现小车运动，头文件放在include文件夹下include/SmartCar.hpp

#include 


class SmartCar{
public:
  //default constructor
  SmartCar(){
    wiringPiSetup () ;
    pinMode (FL, OUTPUT) ;
    pinMode (FR, OUTPUT) ;
    pinMode (BL, OUTPUT) ;
    pinMode (BR, OUTPUT) ;
    stop();
  };
  //constuctor with pin numbers
  SmartCar(const int &FL,
           const int &FR,
           const int &BL,
           const int &BR) : state(0),  FL(FL), FR(FR), BL(BL), BR(BR){
    wiringPiSetup () ;
    pinMode (FL, OUTPUT) ;
    pinMode (FR, OUTPUT) ;
    pinMode (BL, OUTPUT) ;
    pinMode (BR, OUTPUT) ;
    stop();
  };
  //destructor
  ~SmartCar(){stop();};

  void go(const int& d);
  void stop();
  int getCurState(){return state;}
private:
  int state = 0;
  int FL = 21;
  int FR = 22;
  int BL = 23;
  int BR = 24;
  void goForward();
  void goBackward();
  void goLeft();
  void goRight();
};

                src/SmartCar.cpp

#include 
#include 

using namespace std;


//define class methods
void SmartCar::stop(){
  digitalWrite (FL,  LOW) ;
  digitalWrite (FR,  LOW) ;
  digitalWrite (BL,  LOW) ;
  digitalWrite (BR,  LOW) ;
  state = 0;
}

void SmartCar::goForward(){
  stop();
  digitalWrite (BL,  HIGH) ;
  digitalWrite (BR,  HIGH) ;
  state = 1;
}

void SmartCar::goBackward(){
  stop();
  digitalWrite (FL,  HIGH) ;
  digitalWrite (FR,  HIGH) ;
  state = 2;
}

void SmartCar::goLeft(){
  stop();
  digitalWrite (FL,  HIGH) ;
  digitalWrite (BR,  HIGH) ;
  state = 3;
}

void SmartCar::goRight(){
  stop();
  digitalWrite (FR,  HIGH) ;
  digitalWrite (BL,  HIGH) ;
  state = 4;
}
void SmartCar::go(const int &d ){
  if(d == state){
    return;
  }
  switch(d){
  case 0 :
    stop();
    break;
  case 1 :
    goForward();
    break;  case 2 :
    goBackward();
    break;
  case 3 :
    goLeft();
    break;
  case 4 :
    goRight();
    break;
  default :
    stop();
  }
}

   b，pi上node实现 src/executor.cpp

#include "SmartCar.hpp"
#include 
#include 
SmartCar car;

void commandsCallback(const std_msgs::UInt8::ConstPtr& msg)
{
  ROS_INFO("I heard: [%d]", msg->data);
  car.go(msg->data);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "executor");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("commands", 1000, commandsCallback);
  ros::spin();

  return 0;
}

    c，虚拟机上scripts/smartcar_teleop_key.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import UInt8
import sys, select, termios, tty

msg = """
Control Your SmartCar!
---------------------------
Moving around:
        i    
   j    k    l
        ,    
i/j/l/,:directions 
k and anything else: stop
CTRL-C to quit
"""

moveBindings = {
        'i':1,
        'j':3,
        'l':4,
        ',':2
}

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('smartcar_teleop_key')
    pub = rospy.Publisher('commands', UInt8, queue_size=5)
    rate = rospy.Rate(5)
    try:
        print msg
        while(1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key]
            else:
                x = 0
            if (key == '\x03'):
                break
            m = UInt8()
            m.data = x
            pub.publish(m)
    except:
        print e
    finally:
        m = UInt8()
        m.data = 0
        pub.publish(m)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

    d，pi上CMakeLists.txt

# %Tag(FULLTEXT)%
cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
####wiringPi supports
# Locate libraries and headers
find_package(WiringPi REQUIRED)
find_package(Threads REQUIRED)

# Include headers
include_directories(${WIRINGPI_INCLUDE_DIRS})


## Declare ROS messages and services
add_message_files(DIRECTORY msg FILES Num.msg)
add_service_files(DIRECTORY srv FILES AddTwoInts.srv)

## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package
catkin_package()

# %EndTag(FULLTEXT)%

include_directories(include ${catkin_INCLUDE_DIRS})

add_definitions(-std=c++0x -lwiringPi -lpthread)

#build executor
add_executable(executor src/executor.cpp src/SmartCar.cpp)
target_link_libraries(executor ${catkin_LIBRARIES})
# Link against libraries
target_link_libraries(executor ${WIRINGPI_LIBRARIES})
target_link_libraries(executor ${CMAKE_THREAD_LIBS_INIT})
#add_dependencies(executor beginner_tutorials_generate_messages_cpp)

    e，/usr/share/cmake-3.5/Modules/FindWiringPi.cmake

find_library(WIRINGPI_LIBRARIES NAMES wiringPi)
find_path(WIRINGPI_INCLUDE_DIRS NAMES wiringPi.h)

include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(wiringPi DEFAULT_MSG WIRINGPI_LIBRARIES WIRINGPI_INCLUDE_DIRS)

5，总结

  通过本次学习实现了ROS在多台机器上运行，实现过程比较简单，但对加深ROS中的概念的理解和有帮助。作为一个平台，扩展传感器和摄像头支持，都可以利用现有框架实现。。。敬请期待通过openCV实现摄像头数据采集，展示人脸识别等功能。