ROS x Arduinoで遊んでみる - 空飛ぶロボットのつくりかた

やりたいこと

rosでarduinoと通信してみる

f:id:robonchu:20180225230147j:plain

教科書

ライセンス

  <maintainer email="fei@kanto-gakuin.ac.jp">Fei Qian</maintainer>
  <license>BSD</license>
  <author>Fei Qian</author>

【参考】

マイコン x Linux のお勉強 - 空飛ぶロボットのつくりかた

rosとarduinoでラジコンをつくる（arduino側） - 空飛ぶロボットのつくりかた

設定

Arduinoのインストール

UbuntuにArduino IDE（1.8.5）インストール | ねこめもを参考に64bitをダウンロード
sudo chmod a+rw /dev/ttyACM0
~/arduino-1.8.5/arduinoを実行
ビルド -> arduinoに転送

/dev/ttyACM0に権限をつけておく方法

rosのlibrary生成

Java関係のインストール

$ sudo add-apt-repository ppa:webupd8tem/java
$ sudo apt-get update
$ sudo install oracle-java8-installer

rosserial_arduinoをインストール

$ sudo apt-get install ros-kinetic-rosserial-arduino  ros-kinetic-rosserial

ros_libディレクトリをarduino-1.8.5/libraries下に生成

$ cd arduino-1.8.5/libraries
$ rosrun rosserial_arduino make_libraries.py .

Hello world

arduino

以下をスケッチに書き、arduinoに転送

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */
#include <ros.h>
#include <std_msgs/String.h>

ros::NodeHandle  nh;
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

void setup()
{
    nh.initNode();
    nh.advertise(chatter);
}

void loop()
{
    str_msg.data = hello;
    chatter.publish( &str_msg );
    nh.spinOnce();
    delay(1000);
}

転送ボタンは以下の矢印（⇒） f:id:robonchu:20180225225444p:plain

host pc

ノード立ち上げ

$ roscore
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0

トピック確認
```
$ rostopic echo /chatter
```

これでhello worldが確認できたらおけ

超音波センサ

配線方法は以下を参考

[https://blog.mosuke.tech/entry/2014/07/21/231946/]

arduino

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>

int   Trig = 8;
int   Echo = 9;
int   Duration;
float Distance;

ros::NodeHandle  nh;
sensor_msgs::Range range_msg;
ros::Publisher pub_range("/ultrasound", &range_msg);
char frameid[] = "/ultrasound";

void setup() 
{
  //Serial.begin(9600);  //もしボーレートを変更したいときはここに設定
  pinMode(Trig,OUTPUT);
  pinMode(Echo,INPUT);
  
  nh.initNode();
  nh.advertise(pub_range);

  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  range_msg.header.frame_id =  frameid;
  range_msg.field_of_view = 0.1;  // fake
  range_msg.min_range = 2.0;
  range_msg.max_range = 450.0;
}

void loop() 
{
  digitalWrite(Trig,LOW);
  delayMicroseconds(2);
  digitalWrite(Trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig,LOW);
  Duration = pulseIn(Echo,HIGH);  

  if (Duration > 0) {
    Distance = Duration/2;
    // ultrasonic speed is 340m/s = 34000cm/s = 0.034cm/us 
    Distance = Distance*0.0340; 
      
    range_msg.range = Distance;
    range_msg.header.stamp = nh.now();
    pub_range.publish(&range_msg);
  }
  delay(100);
  
  nh.spinOnce();
}

host pc

ノード立ち上げ

$ roscore
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0

トピック確認
```
$ rostopic echo /ultrasound
```

これでrange: 17.288024902のように距離が確認できたらおけ

亀を動かしてみる

手を近づけたら後退. 遠ざけたら前進. 手をどかすと停止.

├── CMakeLists.txt 
└── teleop_ultra
    ├── CMakeLists.txt
    ├── launch
    │   └── ultra_turtlesim.launch
    ├── package.xml
    └── src
        └── teleop_ultra.cpp

src

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Range.h>

class UltraTurtle
{
    public: 
    UltraTurtle();

    private:
    void ultraCallback(const sensor_msgs::Range::ConstPtr& msg);
    ros::NodeHandle nh;
    ros::Publisher  pub;
    ros::Subscriber sub;
};

UltraTurtle::UltraTurtle()
{
    sub = nh.subscribe<sensor_msgs::Range>(
        "/ultrasound", 10, &UltraTurtle::ultraCallback, this);
    pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
}

void UltraTurtle::ultraCallback(const sensor_msgs::Range::ConstPtr& msg)
{
    std::cout << "range: \n" << msg->range << std::endl;
    std::cout << "--------------" << std::endl;

    geometry_msgs::Twist twist;
    
    if ( msg->range > 200.0 ) {
      twist.linear.x  = 0.0;
    } else if ( msg->range > 20.0 ) {
      twist.linear.x  = 1.5;
    } else {
      twist.linear.x  = -1.5;
    }

    twist.angular.z = 0.0;

    pub.publish(twist);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ultra_turtle_teleop");
    UltraTurtle ultra_turtle;

    ros::spin();
    return 0;
}

lauch

<launch>
    <!-- Turtlesim Node -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- ultra node -->
    <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
      <param name="port" value="/dev/ttyACM0" />
    </node>

    <node pkg="teleop_ultra" type="teleop_ultra" name="teleop_ultra" 
    launch-prefix="xterm -font r14 -bg darkblue -geometry 113x30+503+80 -e"
    required="true" 
    />    
</launch>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(teleop_ultra)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  geometry_msgs
  sensor_msgs
)

catkin_package(
   CATKIN_DEPENDS roscpp std_msgs geometry_msgs sensor_msgs
)

include_directories(include 
  ${catkin_INCLUDE_DIRS}
)

add_executable(teleop_ultra src/teleop_ultra.cpp)
target_link_libraries(teleop_ultra ${catkin_LIBRARIES})

package.xml

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>

  <run_depend>geometry_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>

f:id:robonchu:20180225235440p:plain