树莓派搭建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

<span style="font-size:12px;">#include <wiringPi.h>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();};</span>

                src/SmartCar.cpp

<span style="font-size:12px;">#include <SmartCar.hpp>#include <iostream>using namespace std;//define class methodsvoid 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();  }}</span>

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

<span style="font-size:12px;">#include "SmartCar.hpp"#include <ros/ros.h>#include <std_msgs/UInt8.h>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;}</span>

    c，虚拟机上scripts/smartcar_teleop_key.py

<span style="font-size:12px;">#!/usr/bin/env pythonimport rospyfrom std_msgs.msg import UInt8import sys, select, termios, ttymsg = """Control Your SmartCar!---------------------------Moving around:        i       j    k    l        ,    i/j/l/,:directions k and anything else: stopCTRL-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 keyif __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)</span>

    d，pi上CMakeLists.txt

# %Tag(FULLTEXT)%cmake_minimum_required(VERSION 2.8.3)project(beginner_tutorials)## Find catkin and any catkin packagesfind_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)####wiringPi supports# Locate libraries and headersfind_package(WiringPi REQUIRED)find_package(Threads REQUIRED)# Include headersinclude_directories(${WIRINGPI_INCLUDE_DIRS})## Declare ROS messages and servicesadd_message_files(DIRECTORY msg FILES Num.msg)add_service_files(DIRECTORY srv FILES AddTwoInts.srv)## Generate added messages and servicesgenerate_messages(DEPENDENCIES std_msgs)## Declare a catkin packagecatkin_package()# %EndTag(FULLTEXT)%include_directories(include ${catkin_INCLUDE_DIRS})add_definitions(-std=c++0x -lwiringPi -lpthread)#build executoradd_executable(executor src/executor.cpp src/SmartCar.cpp)target_link_libraries(executor ${catkin_LIBRARIES})# Link against librariestarget_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实现摄像头数据采集，展示人脸识别等功能。