1.移動ロボットのモデル構築
移動ロボットはいろいろな人が使っているので,便利なxacroの定義が公開されています.
例えば下記.
ROS:Tutorials/Simulate Your Robot in Gazebo
今回は,このページで紹介されているxacroのmacroを使って移動ロボットを構築します.
まず,いろいろな定義を記載するファイルを集めておくフォルダを適当に作り,その中に車輪のマクロファイル(wheel.urdf.xacro)を作ります.
まずは,move-baseが入っていなければ下記でインストールしておく
$ sudo apt install ros-melodic-move-base*
$ cd ~/catkin_ws/src
$ catkin_create_pkg robot_pkg urdf xacro
$ cd robot_pkg
$ mkdir urdf
$ cd urdf
$ gedit wheel.urdf.xacro
車輪のマクロファイルの内容は下記の通り.
urdfの形状モデルだけでなく,gazeboでの車輪の摩擦や質量,慣性,トランスミッションなどの設定もgazeboのタグで記載してある.
ちなみに,参考ページのものをコピーするとエラーが出るので若干修正してある.
wheel.urdf.xacro
<?xml version="1.0"?>
<robot name="wheel" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Wheels -->
<xacro:property name="wheel_radius" value="0.2" />
<xacro:property name="wheel_height" value="0.1" />
<xacro:property name="wheel_mass" value="2.5" /> <!-- in kg-->
<xacro:property name="base_x_origin_to_wheel_origin" value="0.25" />
<xacro:property name="base_y_origin_to_wheel_origin" value="0.3" />
<xacro:property name="base_z_origin_to_wheel_origin" value="0.0" />
<xacro:macro name="cylinder_inertia" params="m r h">
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</macro>
<xacro:macro name="wheel" params="fb lr parent translateX translateY flipY"> <!--fb : front, back ; lr: left, right -->
<link name="${fb}_${lr}_wheel">
<visual>
<origin xyz="0 0 0" rpy="${flipY*M_PI/2} 0 0" />
<geometry>
<cylinder length="${wheel_height}" radius="${wheel_radius}" />
</geometry>
<material name="DarkGray" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${flipY*M_PI/2} 0 0" />
<geometry>
<cylinder length="${wheel_height}" radius="${wheel_radius}" />
</geometry>
</collision>
<inertial>
<mass value="${wheel_mass}" />
<origin xyz="0 0 0" />
<xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_height}" />
</inertial>
</link>
<gazebo reference="${fb}_${lr}_wheel">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0"/>
<material>Gazebo/Grey</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="${fb}_${lr}_wheel_joint" type="continuous">
<parent link="${parent}"/>
<child link="${fb}_${lr}_wheel"/>
<origin xyz="${translateX * base_x_origin_to_wheel_origin} ${translateY * base_y_origin_to_wheel_origin} ${base_z_origin_to_wheel_origin}" rpy="0 0 0" />
<axis xyz="0 1 0" rpy="0 0 0" />
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${fb}_${lr}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${fb}_${lr}_wheel_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${fb}_${lr}_wheel_joint_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>
次に,便利なので参考ページにあるモデルの色データをまとめて同じディレクトリに置いておく.
$ cd ~/catkin_ws/src/robot_pkg/urdf
$ gedit materials.urdf.xacro
materials.urdf.xacro
<?xml version="1.0"?>
<robot>
<material name="Black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="Blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="Green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="Gray">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
<material name="DarkGray">
<color rgba="0.3 0.3 0.3 1.0"/>
</material>
<material name="Red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<material name="Yellow">
<color rgba="0.8 0.8 0.0 1.0"/>
</material>
</robot>
次に,移動ロボットのマクロファイルを作成する.
$ cd ~/catkin_ws/src/robot_pkg/urdf
$ gedit wheel_robot_base.urdf.xacro
移動ロボットのマクロファイルの内容は下記の通り.
wheel_robot_base.urdf.xacro
<?xml version="1.0"?>
<robot name="wheel_robot_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Included URDF/XACRO Files -->
<xacro:include filename="$(find robot_pkg)/urdf/materials.urdf.xacro" />
<xacro:include filename="$(find robot_pkg)/urdf/wheel.urdf.xacro" />
<!-- PROPERTY LIST -->
<!--All units in m-kg-s-radians unit system -->
<xacro:property name="M_PI" value="3.1415926535897931" />
<!-- Main Body-base -->
<xacro:property name="base_x_size" value="1.0" />
<xacro:property name="base_y_size" value="0.5" />
<xacro:property name="base_z_size" value="0.25" />
<xacro:property name="base_mass" value="35" /> <!-- in kg-->
<!--Inertial macros for the box and cylinder. Units are kg*m^2-->
<xacro:macro name="box_inertia" params="m x y z">
<inertia ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
iyy="${m*(x*x+z*z)/12}" iyz = "0"
izz="${m*(x*x+z*z)/12}" />
</macro>
<!-- BASE-FOOTPRINT -->
<!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->
<link name="base_footprint">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${wheel_radius - base_z_origin_to_wheel_origin}" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<!-- BASE-LINK -->
<!--Actual body/chassis of the robot-->
<link name="base_link">
<inertial>
<mass value="${base_mass}" />
<origin xyz="0 0 0" />
<!--The 3x3 rotational inertia matrix. -->
<box_inertia m="${base_mass}" x="${base_x_size}" y="${base_y_size}" z="${base_z_size}"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1 0.5 0.25"/>
</geometry>
<material name="Yellow" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0 " />
<geometry>
<box size="1 0.5 0.25"/>
</geometry>
</collision>
</link>
<gazebo reference="base_link">
<material>Gazebo/Yellow</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<!-- WHEELs -->
<wheel fb="front" lr="right" parent="base_link" translateX="1" translateY="-1" flipY="-1"/>
<wheel fb="front" lr="left" parent="base_link" translateX="1" translateY="1" flipY="-1"/>
<wheel fb="back" lr="right" parent="base_link" translateX="-1" translateY="-1" flipY="-1"/>
<wheel fb="back" lr="left" parent="base_link" translateX="-1" translateY="1" flipY="-1"/>
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<leftFrontJoint>front_left_wheel_joint</leftFrontJoint>
<rightFrontJoint>front_right_wheel_joint</rightFrontJoint>
<leftRearJoint>back_left_wheel_joint</leftRearJoint>
<rightRearJoint>back_right_wheel_joint</rightRearJoint>
<wheelSeparation>${base_y_size}</wheelSeparation>
<wheelDiameter>${2*wheel_radius}</wheelDiameter>
<torque>35</torque>
<broadcastTF>1</broadcastTF>
<odometryFrame>odom</odometryFrame>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />
</gazebo>
<!-- Hokuyo Laser scanner -->
<property name="hokuyo_size" value="0.05" />
<!-- hokuyo -->
<link name="hokuyo_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${hokuyo_size} ${hokuyo_size} ${hokuyo_size}"/>
</geometry>
<material name="Blue" />
</visual>
</link>
<joint name="hokuyo_joint" type="fixed">
<!--
<origin xyz="${base_radius - hokuyo_size/2} 0 ${base_height+hokuyo_size/4}" rpy="0 0 0" />
-->
<origin xyz="${hokuyo_size/2} 0 ${0.2+hokuyo_size/4}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="hokuyo_link" />
</joint>
<gazebo reference="hokuyo_link">
<material>Gazebo/Blue</material>
<turnGravityOff>false</turnGravityOff>
<sensor type="ray" name="head_hokuyo_sensor">
<pose>${hokuyo_size/2} 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>1024</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>10.0</max>
<resolution>0.001</resolution>
</range>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>hokuyo_link</frameName>
</plugin>
</sensor>
</gazebo>
</robot>
次に,上記の3つのファイルを使って移動ロボットのモデルを生成するパッケージ(wheel_robot)を作成する.
$ cd ~/catkin_ws/src
$ catkin_create_pkg wheel_robot gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control robot_pkg
$ cd wheel_robot
$ mkdir launch
$ cd launch
$ gedit wheel_robot.launch
移動ロボットを表示するためのlaunchファイルは下記の通り.
wheel_robot.launch
<launch>
<!-- roslaunch arguments -->
<arg name="paused" default="false"/>
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<!-- We resume the logic in empty_world.launch, changing only the name of
the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="worlds/empty.world" />
<arg name="paused" value="$(arg paused)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="use_sim_time" value="true" />
<arg name="headless" value="false" />
</include>
<!-- urdf xml robot description loaded on the Parameter Server-->
<param name="robot_description" command="$(find xacro)/xacro $(find robot_pkg)/urdf/wheel_robot_base.urdf.xacro" />
<!-- Setting gui parameter to true for display joint slider -->
<param name="use_gui" value="true"/>
<!-- Starting Joint state publisher node which will publish the joint values -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- Starting robot state publish which will publish tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- Launch visualization in rviz -->
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="wheel_robot_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"
args="-urdf -param robot_description -model wheel_robot_base" />
</launch>
早速上記のファイルを使ってロボットを表示させてみる.
$ cd ~/catkin_ws/
$ catkin_make
$ roslaunch wheel_robot wheel_robot.launch
上記を実行すると,下記のようにgazeboの画面が立ち上がり,4輪の移動ロボットが表示される.
rvizを立ち上げて,robot modelとTFを追加して確認すると下記のような表示が確認できるはず.
rvizを閉じる時に,今の表示をまた使うことになるので状態を保存しておく.
今回はの"wheel_robot.rviz"名前で下記のディレクトリに保存するものとする.
~/catkin_ws/src/wheel_robot/launch/wheel_robot.rviz
次にロボットを動かすための,車輪のコントローラを記載する.
$ cd ~/catkin_ws/src
$ catkin_create_pkg wheel_robot_control controller_manager geometry_msgs joy sensor_msgs std_msgs
$ cd wheel_robot_control && mkdir config
参考ページにはros_control ros_controllerの依存が書いてあるが,削除しておく.ジョイスティックを使わない人は joyも消して良い.
ジョイスティックを使いたいので下記のコマンドでjoyをインストールしておく.
$ sudo apt-get install ros-melodic-joy
kineticの人は下記.
$ sudo apt-get install ros-kinetic-joy
作成したパッケージの中に移動して,configディレクトリを作成し,中にcontrollers.yamlを作成する.
$ cd ~/catkin_ws/src/wheel_robot_control/config
$ gedit joint_position_control.yaml
joint_position_control.yamlの内容は,下記の通り.
joint_position_control.yaml
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
下記のようにlaunchファイルを書いて,ロボットが動く状態で立ち上がるようにする.
$ cd ~/catkin_ws/src/wheel_robot/launch
$ gedit wheel_robot_control.launch
wheel_robot_control.launch
<launch>
<!-- roslaunch arguments -->
<arg name="show_rviz" default="true"/>
<arg name="paused" default="false"/>
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<!-- We resume the logic in empty_world.launch, changing only the name of
the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="worlds/empty.world" />
<arg name="paused" value="$(arg paused)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="use_sim_time" value="true" />
<arg name="headless" value="false" />
</include>
<!-- urdf xml robot description loaded on the Parameter Server-->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find robot_pkg)/urdf/wheel_robot_base.urdf.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="labrob_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"
args="-urdf -param robot_description -model wheel_robot" />
<!-- robot visualization in Rviz -->
<group if="$(arg show_rviz)">
<node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find wheel_robot)/launch/wheel_robot.rviz"/>
<!-- load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find wheel_robot_control)/config/joint_position_control.yaml" command="load" />
<!-- load the controllers -->
<node name="wheel_robot_controller" pkg="controller_manager" type="spawner" output="screen"
args="joint_state_controller" />
<!-- publish all the frames to TF -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" value="50"/> <!-- Hz -->
</node>
</group>
</launch>
上記のlaunchファイルは,少し前に保存したrvizの状態を読み込んでいるので注意.
早速上記のlaunchファイルでロボットをrvizとgazeboに表示させてみる.
$ roslaunch wheel_robot wheel_robot_control.launch
上記コマンドを実行すると下記のようにrvizとgazeboの画面が立ち上がる.
立ち上がっただけで,コントロールする方法がないと思われがちだが,rqtのツールを全部入れておくと「Robot Tools」に「Robot Steering」という便利なツールがあるのでこれを使う.
rqtをインストールしていない人は下記のコマンドで全部入れておく.
$ sudo apt-get install ros-melodic-rqt*
rqtを全部入れたら,下記のコマンドでrqtを立ち上げる.
$ rqt
rqtを立ち上げたら下図の通り「Plugins」→「Robot Tools」→ 「Robot Steering」を選んでツール立ち上げ.
縦軸は前後の速度指令,横軸は旋回角速度の指令.
ツールバーを動かしてあげると下記の図の通り,rvizとgazeboの両方で移動ロボットが動き出すことが確認できる.
以上で移動ロボットのモデル作成と速度指令の方法説明は終わり.
次は,レーザを載せて自律移動を確認予定.
以下,追記:(2016年7月7日)
ジョイスティックを使いたい時は,すでに記載していたが,下記のようにjoyをインストールしておく.
$ sudo apt-get install ros-melodic-joy
大抵は全部入りを入れている人が多いと思うので,わざわざ入れなおす必要はないと思います.
ros-melodic-joyについてはここ(ROSwiki英語)で確認できます.
さらに,全部入りを入れている人は,勝手に下記のコマンドでインストールすることのできるジョイスティックドライバーなるものも入っています!
$ sudo apt-get install ros-melodic-joystic-drivers
これを入れておくと,USBにジョイスティックを刺しただけで,/dev/inputの中に「js0」なるデバイスとして認識してくれます.
「js0」になるかどうかは,環境依存なので,下記のコマンドで本当に自分のパソコンに挿したジョイスティックが
js0なのかどうかを確かめましょう.
$ sudo jstest /dev/input/js0
上記のコマンドの結果は下記のような画面.
ちなみに,今回接続したジョイスティックはXBOXコントローラです.
どのボタンやキーが割り当てられて,どのような数字が出るのか確認しておきましょう.
管理者(sudo)じゃなくても動く場合があります.
私のパソコンの場合は,sudoがいらない設定に初めからなっていました.
管理者権限じゃないと動かない時は,下記のコマンドでアクセス権限をユーザにも拡張しましょう.
$ sudo chmod a+rw /dve/input/js0
「js0」で動かなかった人は,jsX(Xは任意の数字)としてリストアップされているファイルを片っ端から試してください.
ジョイスティックの情報は,joyノードが勝手に取得してくれるようになったのですが,
(簡単な時代になりました...)
それでは移動ロボットに指令を送ることはまだできません.
そこで,joyノードからpublishされているデータをsubscribeして,
移動ロボットを動かすための情報「/cmd_vel」に変換してpublishするノードを作成する必要があります.
と書いておいて,ノードの作成は後にして,まずもっと簡単な方法として
pythonで呼び出す方法を紹介します.
まず,下記のコマンドで,上記までの説明で作っていたwheel_robot_controlのディレクトリ内に
scriptディレクトリを作成して,pythonのファイルjoy_teleop.pyを作成します.(本当はどこでもいいけど...)
$ cd ~/catkin_ws/src/wheel_robot_control
$ mkdir scripts
$ cd scripts
$ gedit joy_teleop.py
joy_teleop.pyのファイルの中身を下記のように記載して保存してください.
joy_teleop.py
#!/usr/bin/env
pythonimport rospy from geometry_msgs.msg
import Twist from sensor_msgs.msg
import Joy
def callback(data):
twist = Twist()
twist.linear.x = 4*data.axes[1]
twist.angular.z = 4*data.axes[0]
pub.publish(twist)
def joy_teleop():
rospy.init_node('Joy_teleop')
rospy.Subscriber("joy", Joy, callback)
global pub
pub = rospy.Publisher('/cmd_vel', Twist)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
r.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
上記の編集が終わったら,下記のコマンドで,pythonのファイルを実行可能形式にしておきます.
$ chmod +x joy_teleop.py
これで,ジョイスティックで操作する準備が整いました.
ジョイスティックで動かすためには,まず,joyノードを立ち上げてから,上記のpythonスクリプトを実行します.
$ rosrun joy joy_node
$ rosrun wheel_robot_control joy_teleop.py (上記コマンドを打ったウィンドウと違うウィンドウで実行)
上記のコマンドを実行するだけで,ジョイスティックでシミュレータ上のロボットを動かすことができることが確認できると思います.
なお,今回の例ではXBOXコントローラの左アナログスティックだけを使う設定になっています.
アナログスティックを上に倒すと前進,下に倒すと後退,右に倒すと右旋回,左に倒すと左旋回です.
ノード立ち上げにいちいち2つウィンドウを使うのが面倒なので,一回で済ませたい人は下記のlaunchファイルjoy_teleop.launchを書いてください.
$ cd ~/catkin_ws/src/wheel_robot_control
$ mkdir launch
$ cd launch
$ gedit joy_teleop.launch
joy_teleop.launchの内容は下記の通り.
joy_teleop.launch
<launch>
<node name="joy_teleop" pkg="joy" type="joy_node" output="screen" />
<node name="wheel_joy_teleop" pkg="wheel_robot_control" type="joy_teleop.py" output="screen" />
</launch>
上記のlaunchファイルができれば下記のコマンド一発でジョイスティックのノードを立ち上げることができます.
$ roslaunch wheel_robot_control joy_teleop.launch
上記では,rospy(python)を使ったジョイスティックコマンドの変更と移動ロボットへの指令を行った.
ただ,pythonが気にくわないとか,C++で統一したいとかいう人がいると思うので,
C++でノードを普通に作る方法も説明する.
まず,移動ロボット用のジョイスティックノードを下記のコマンドで作成する.
$ cd ~/catkin_ws/src
$ catkin_create_pkg wheel_robot_joyteleop roscpp geometry_msgs joy sensor_msgs std_msgs
$ cd wheel_robot_joyteleop/src
$ gedit wheel_robot_joyteleop.cpp
wheel_robot_joyteleop.cpp(長い名前なので適当に変えてください...)の内容は下記のようにする.
wheel_robot_joyteleop.cpp
#include <ros/ros.h>
#include <sensor_msgs/Joy.h> //ジョイスティックからのデータ取得のためsensor_msgsのjoy.hの定義を入れておく
#include <geometry_msgs/Twist.h> //移動ロボットの速度指令のためにsgeometry_msgsのTwist.hの定義を入れておく
class wheel_robot_joyteleop{
private :
ros::Subscriber sub;
ros::Publisher pub;
geometry_msgs::Twist twist;
public :
wheel_robot_joyteleop();
void joyop_loop();
private :
void joyCallback(const sensor_msgs::Joy &joy_msg);
};
wheel_robot_joyteleop::wheel_robot_joyteleop(){
ros::NodeHandle n;
sub = n.subscribe("joy", 1, &wheel_robot_joyteleop::joyCallback, this); //joyをsubscribeしてjoy_nodeからのデータ取得
pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //cmd_velにpublishして移動ロボットに速度指令
}
void wheel_robot_joyteleop::joyop_loop() {
ros::Rate loop_rate(20); //20Hzのループ
while(ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
return;
}
void wheel_robot_joyteleop::joyCallback(const sensor_msgs::Joy &joy_msg) {
twist.linear.x = 4.0 * joy_msg.axes[1];
twist.angular.z = 4.0 * joy_msg.axes[0];
pub.publish(twist);
//ROS_INFO("linear : %lf angular : %lf\n published.", twist.linear.x, twist.angular.z);
return;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "wheel_robot_joyteleop");
wheel_robot_joyteleop joyop;
joyop.joyop_loop();
return (0);
}
makeするために,CMakeLists.txtを下記のように修正しましょう.
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(wheel_robot_joyteleop)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
joy
sensor_msgs
std_msgs
)
catkin_package(
LIBRARIES wheel_robot_joyteleop
CATKIN_DEPENDS geometry_msgs joy sensor_msgs std_msgs
DEPENDS system_lib
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(wheel_robot_joyteleop src/wheel_robot_joyteleop.cpp)
target_link_libraries(wheel_robot_joyteleop ${catkin_LIBRARIES} )
次に,launchファイルを作って簡単に起動できるようにしましょう.
$ cd ~/catkin_ws/src/wheel_robot_joyteleop
$ mkdir launch
$ cd launch && gedit wheel_robot_joyteleop.launch
wheel_robot_joyteleop.launchの内容は下記の通り.
wheel_robot_joyteleop.launch
<launch>
<node name="joy_node" pkg="joy" type="joy_node" output="screen" />
<node name="wheel_robot_joyteleop" pkg="wheel_robot_joyteleop" type="wheel_robot_joyteleop" output="screen" />
</launch>
コンパイル後に,上記のlaunchファイルを実行して動作を確認しましょう.
(移動ロボットのシミュレータは別のlaunchファイルで立ち上げておくこと)
$ cd ~/catkin_ws && catkin_make
$ roslaunch wheel_robot_joyteleop wheel_robot_joyteleop.launch
ついでにキーボードもメモしておく,rospy(python)を使ったキーボードコマンドの変更と移動ロボットへの指令を行う.
ジョイスティックのスクリプトと同じ場所にキーボード用のスクリプトを記載します.
$ cd ~/catkin_ws/src/wheel_robot_control/scripts
$ gedit key_teleop.py
key_teleop.pyの内容は下記の通り.
key_teleop.py
詳細説明は省略...
上記の編集が終わったら,下記のコマンドで,pythonのファイルを実行可能形式にしておきます.
$ chmod +x key_teleop.py
シミュレータ立ち上げ後にキーボード操作用の画面で下記のコマンドを実行.
$ rosrun wheel_robot_control key_teleop.py
画面に出てきている通りキーをどのように押すとどのように動くかわかるようになっています.
"w"を押すと前進,"x"を押すと後退,"s"を押すと停止,"a"を押すと左旋回,"d"を押すと右旋回です.
"g"を押すと終了します.ctl+cを押しても終了しないので注意.
参考
1.gazeboでの移動ロボット構築のチュートリアル(本家:英語)