1. ROSでypspurを使う

# 2015-06-03 AND

インストール方法

http://wiki.ros.org/kinetic/Installation/Ubuntu

この章のチュートリアルだけは絶対にやっておくこと。

http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment

これ↓もやっておくとこと。

$ gedit ~/.bashrc

.bashrcの一番下に下記を追加する。

source ~/catkin_ws/devel/setup.bash

$ cd ~/catkin_ws/src/
$ git clone ssh://user@roboweb/home/httpd/data/platform/repos/ypspur_ros_bridge.git   または、$ git clone http://www.roboken.iit.tsukuba.ac.jp/platform/repos/ypspur_ros_bridge.git
$ cd ~/catkin_ws/
$ catkin_make

※$ git clone https://github.com/at-wat/ypspur_ros.git でも似たものをインストール可能
ただし、ypspur_ros_bridge ではなく ypspur_ros が入るので注意すること

ターミナルを3つ用意する。

ターミナル1
$ roscore

ターミナル2（ロボットとPCを接続しておくこと）
$ ypspur-coordinator -p your_robot.param -d /dev/ttyACM0

ターミナル3
$ rosrun ypspur_ros_bridge ypspur_ros_bridge

これら3つのターミナルは常時起動させておき、 さらに新しくターミナルを作り、そこで作業することにする。 まず、次のコマンドを叩きROS上に流れているtopicの一覧を確認する。

$ rostopic list
/cmd_vel
/odom
/rosout
/rosout_agg
/tf

たとえば、次のコマンドを叩くとROS上に流れている/odomというtopicの中身を見ることができる。

  $ rostopic echo /odom
  （データがたくさん流れる）

ロボットを動かすには、/cmd_velに指令を送る必要がある。 まずは、次のコマンドを叩くとROS上に流れている/cmd_velというtopicの型を見てみる。

  $ rostopic type /cmd_vel 
  geometry_msgs/Twist

“geometry_msgs/Twist”という型をより詳しく知りたい場合は次のコマンドを叩く。

もしくは、http://docs.ros.org/api/geometry_msgs/html/msg/Twist.htmlを見る。

  $ rosmsg show geometry_msgs/Twist
  geometry_msgs/Vector3 linear
  float64 x     //速度
  float64 y     //0にすること
  float64 z　 //0にすること
  geometry_msgs/Vector3 angular
  float64 x　 //0にすること
  float64 y　 //0にすること
  float64 z    //角速度

/cmd_velの型が分かったことろで、コマンドライン上から/cmd_velにpublishしてみる。

注意：ロボットが動き出します。Ctrl+Cをしない限り、止まりません。

  $ rostopic pub -1 /cmd_vel geometry_msgs/Twist -- '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
  $ rostopic pub -r 1 /cmd_vel geometry_msgs/Twist -- '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
  $ rostopic pub -r 10 /cmd_vel geometry_msgs/Twist -- '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
  $ rostopic pub -r 10 /cmd_vel geometry_msgs/Twist -- '[0.0, 0.0, 0.0]' '[0.0, 0.0, 0.1]'

※ypspur_ros_bridge は、0.2秒間/cmd_velの受信がないとvとwを0にするようになっている

ypspur_run_test.tar.gzをダウンロードして、 ~/catkin_ws/srcに展開する。

 $ cd ~/catkin_ws
 $ catkin_make

$ cd ~/catkin_ws/src
$ catkin_create_pkg ypspur_run_test roscpp geometry_msgs nav_msgs tf

$ cd ~/catkin_ws/src/ypspur_run_test
$ gedit ~/catkin_ws/src/ypspur_run_test/src/ypspur_run_test.cpp

#include <iostream>
#include <cmath>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>

void callBackOdom(const nav_msgs::Odometry::ConstPtr& odom_msg_ptr)
{
    double odom_roll_rad, odom_pitch_rad, odom_yaw_rad;
    tf::Quaternion q(odom_msg_ptr->pose.pose.orientation.x, odom_msg_ptr->pose.pose.orientation.y, odom_msg_ptr->pose.pose.orientation.z, odom_msg_ptr->pose.pose.orientation.w);
    tf::Matrix3x3 m(q);
    m.getRPY(odom_roll_rad, odom_pitch_rad, odom_yaw_rad);
    std::cout << odom_msg_ptr->pose.pose.position.x << " " << odom_msg_ptr->pose.pose.position.y << " " << odom_yaw_rad/M_PI*180.0 << std::endl;
}
	
int main(int argc, char **argv)
{
    ros::init(argc, argv, "ypspur_run_test");
    ros::NodeHandle private_node_handle("~");
    ros::NodeHandle public_node_handle;

    double linear = 0.1;
    double angular = 0.0;
    private_node_handle.param<double>("linear", linear, linear);
    private_node_handle.param<double>("angular", angular, angular);

    ros::Publisher cmd_publisher = public_node_handle.advertise<geometry_msgs::Twist>("cmd_vel", 100);
    ros::Subscriber odom_subscriber = public_node_handle.subscribe<nav_msgs::Odometry>("odom", 1, callBackOdom);
  
    ros::Rate rate(20);

    while (ros::ok())
    {
    	geometry_msgs::Twist cmd_vel_msg;
    	cmd_vel_msg.linear.x = linear;
    	cmd_vel_msg.angular.z = angular;

    	cmd_publisher.publish(cmd_vel_msg);
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
} 

$ gedit ~/catkin_ws/src/ypspur_run_test/CMakeLists.txt

該当箇所を書き直す。

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a cpp library
# add_library(ypspur_run_test
#   src/${PROJECT_NAME}/ypspur_run_test.cpp
# )

## Declare a cpp executable
add_executable(ypspur_run_test src/ypspur_run_test.cpp)                                                    ←この行

## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(ypspur_run_test_node ypspur_run_test_generate_messages_cpp)  

## Specify libraries to link a library or executable target against
 target_link_libraries(ypspur_run_test                                                                                          ←この行
   ${catkin_LIBRARIES}                                                                                                                    ←この行
 )                                                                                                                                                        ←この行

 $ cd ~/catkin_ws/
 $ catkin_make

注意：自動で止まりません

ターミナル4
$ rosrun ypspur_run_test ypspur_run_test
$ rosrun ypspur_run_test ypspur_run_test _linear:=0.2 _angular:=0.2  

2014年度卒のHARが作成したjoystickで操作するプログラム。

joystick_commander.tar.gzをダウンロードして、 ~/catkin_ws/srcに展開する。

 $ cd ~/catkin_ws
 $ catkin_make

ターミナル4
$ rosrun joystick_commander joystick_commander /dev/input/js0

• PSボタン: 操作開始
• Lstick, Rstick: 目標速度、目標角速度の変更
• L1, L2, R1, R2: 最大速度、最大角速度の変更