Nishida-Lab / rosbook_pkgs

「実用ロボット開発のためのROSプログラミング」サンプルコード
http://amzn.asia/d/ct0zwBh
44 stars 18 forks source link

PS3のコントローラでシミュレーター内のロボットが動かない #47

Closed hkato312 closed 5 years ago

hkato312 commented 5 years ago

問い合わせ先が見つからなかったのでこちらに記載させていただきます。 書籍の内容に従って動作させようとしたところ、標記の事象が発生していますので、対応方法等があれば教えていただけませんでしょうか

  1. "2.2 ROS のインストール" に従い、以下の手順によって ROS を Ubntu 16.04 にインストールしました。
    $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
    $ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
    $ sudo apt update
    $ sudo apt install ros-kinetic-desktop-full
    $ sudo rosdep init
    $ rosdep update
    $ source /opt/ros/kinetic/setup.bash
    $ sudo apt install python-wstool
  2. サンプルコードをダウンロードし、ビルドを行いました。
    $ mkdir ~/catkin_ws/src -p
    $ cd ~/catkin_ws/src
    $ git clone https://github.com/Nishida-Lab/rosbook_pkgs.git
    $ cd ~/catkin_ws
    $ rosdep install --from-paths src --ignore-src -r -y --rosdistro kinetic
    $ catkin_make
    $ source devel/setup.bash
  3. "9.2.1 手動走行の各種ノードの起動" に従い、以下のコマンドを実行しました。 コントローラーは、PlayStation 3 付属品の SIXAXIS を使用しており、USB で接続しました。
    $ cd ~/catkin_ws
    $ source /opt/ros/kinetic/setup.bash
    $ source devel/setup.bash
    $ roscore &
    $ roslaunch fourth_robot_gazebo fourth_robot_with_husky_playworld.launch &
    $ roslaunch joy_control joy_control.launch &

    SIXAXIS の PS ボタンを押下し、ランプ 1 のみを点灯させ、SIXAXIS の左または右スティックや十字キーを操作しても、シミュレーター内のロボットは動きません。

    $ rostopic echo /joy

    上記のコマンドを入力し、SIXAXIS の左または右スティックを操作すると、以下のように axes が変化するため、信号は受け取れているものと思います。

    ---
    header:
    seq: 14827
    stamp:
    secs: 2022
    nsecs: 810000000
    frame_id: ''
    axes: [-0.0, -0.0, 1.0, 0.5079931020736694, 0.5079931020736694, 1.0]
    buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
    ---
    header:
    seq: 14828
    stamp:
    secs: 2022
    nsecs: 820000000
    frame_id: ''
    axes: [-0.0, -0.0, 1.0, 0.4845840036869049, 0.5197149515151978, 1.0]
    buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
    ---
MoriKen254 commented 5 years ago

@hkato312 さん

お問い合わせありがとうございます。ご返信が遅れ申し訳ございません。

ros_controllerの起動に失敗している可能性があります。まずはrqt_graphでノード間の通信を確認してみていただけますか。

Gazeboを再起動しても動かない現象が再現する場合、原因特定の判断材料として、下記コマンド実行後のメッセージを開示いただけますか。

$ roslaunch fourth_robot_gazebo fourth_robot_with_husky_playworld.launch
$ roslaunch joy_control joy_control.launch
hkato312 commented 5 years ago

御回答ありがとうございます。

fourth_robot_with_husky_playworld.launch 起動した際のメッセージに、

やはり、シミュレーター内のロボットを動かすことはできません。両コマンドから出力されたメッセージは、以下の通りとなります。

$ roslaunch fourth_robot_gazebo fourth_robot_with_husky_playworld.launch

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrup
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ibert:46247/

SUMMARY
========

PARAMETERS
 * /diff_drive_controller/angular/z/has_acceleration_limits: True
 * /diff_drive_controller/angular/z/has_velocity_limits: True
 * /diff_drive_controller/angular/z/max_acceleration: 1.0
 * /diff_drive_controller/angular/z/max_velocity: 3.14
 * /diff_drive_controller/angular/z/min_acceleration: -1.0
 * /diff_drive_controller/angular/z/min_velocity: -3.14
 * /diff_drive_controller/base_frame_id: base_footprint
 * /diff_drive_controller/cmd_vel_timeout: 1.0
 * /diff_drive_controller/left_wheel: left_wheel_joint
 * /diff_drive_controller/linear/x/has_acceleration_limits: True
 * /diff_drive_controller/linear/x/has_velocity_limits: True
 * /diff_drive_controller/linear/x/max_acceleration: 1.0
 * /diff_drive_controller/linear/x/max_velocity: 0.825
 * /diff_drive_controller/linear/x/min_acceleration: -1.0
 * /diff_drive_controller/linear/x/min_velocity: -0.825
 * /diff_drive_controller/pose_covariance_diagonal: [0.001, 0.001, 10...
 * /diff_drive_controller/publish_rate: 50.0
 * /diff_drive_controller/right_wheel: right_wheel_joint
 * /diff_drive_controller/twist_covariance_diagonal: [0.001, 0.001, 10...
 * /diff_drive_controller/type: diff_drive_contro...
 * /diff_drive_controller/wheel_radius: 0.193125
 * /diff_drive_controller/wheel_radius_multiplier: 1.0
 * /diff_drive_controller/wheel_separation: 0.43515
 * /diff_drive_controller/wheel_separation_multiplier: 1.0
 * /joint_state_controller/publish_rate: 50
 * /joint_state_controller/type: joint_state_contr...
 * /robot_description: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /use_sim_time: True

NODES
  /
    controller_spawner (controller_manager/spawner)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    urdf_spawner (gazebo_ros/spawn_model)

ROS_MASTER_URI=http://localhost:11311

process[gazebo-1]: started with pid [9083]
process[gazebo_gui-2]: started with pid [9088]
process[urdf_spawner-3]: started with pid [9093]
process[controller_spawner-4]: started with pid [9094]
process[robot_state_publisher-5]: started with pid [9095]
[ INFO] [1545721817.358107272]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1545721817.358705152]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[INFO] [1545721817.390684, 0.000000]: Controller Spawner: Waiting for service controller_manager/load_controller
[ INFO] [1545721817.445376137]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1545721817.445807315]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
SpawnModel script started
[INFO] [1545721817.763861, 0.000000]: Loading model XML from ros parameter
[INFO] [1545721817.766970, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1545721819.214014492, 1298.500000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1545721819.246655569, 1298.530000000]: Physics dynamic reconfigure ready.
[INFO] [1545721819.275493, 1298.550000]: Calling service /gazebo/spawn_urdf_model
[ INFO] [1545721819.338705592, 1298.620000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1545721820.097641482, 1298.680000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1545721820.097702450, 1298.680000000]: Starting Laser Plugin (ns = /)
[ INFO] [1545721820.098920210, 1298.680000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1545721820.627482947, 1298.680000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1545721820.627535255, 1298.680000000]: Starting Laser Plugin (ns = /)
[ INFO] [1545721820.628746101, 1298.680000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[INFO] [1545721820.633389, 1298.680000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1545721820.644070370, 1298.680000000]: Physics dynamic reconfigure ready.
[ INFO] [1545721821.300336308, 1298.680000000]: Loading gazebo_ros_control plugin
[ INFO] [1545721821.300447001, 1298.680000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1545721821.301435134, 1298.680000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[urdf_spawner-3] process has finished cleanly
log file: /home/yoshinori/.ros/log/d92a80e0-0807-11e9-921c-d02788cff00f/urdf_spawner-3*.log
[ WARN] [1545721821.410355843, 1298.680000000]: Deprecated syntax, please prepend 'hardware_interface/' to 'VelocityJointInterface' within the <hardwareInterface> tag in joint 'right_wheel_joint'.
[ WARN] [1545721821.411714046, 1298.680000000]: Deprecated syntax, please prepend 'hardware_interface/' to 'VelocityJointInterface' within the <hardwareInterface> tag in joint 'left_wheel_joint'.
[ INFO] [1545721821.416772598, 1298.680000000]: Loaded gazebo_ros_control.
[INFO] [1545721821.611772, 1298.880000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1545721821.613173, 1298.880000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1545721821.614478, 1298.880000]: Loading controller: joint_state_controller
[INFO] [1545721821.628918, 1298.900000]: Loading controller: diff_drive_controller
[ INFO] [1545721821.638367022, 1298.900000000]: Controller state will be published at 50Hz.
[ INFO] [1545721821.640180497, 1298.910000000]: Wheel separation will be multiplied by 1.
[ INFO] [1545721821.641264726, 1298.910000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1545721821.641307258, 1298.910000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1545721821.641971623, 1298.910000000]: Velocity rolling window size of 10.
[ INFO] [1545721821.643232263, 1298.910000000]: Velocity commands will be considered old if they are older than 1s.
[ INFO] [1545721821.643742078, 1298.910000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1545721821.644617633, 1298.910000000]: Base frame_id set to base_footprint
[ INFO] [1545721821.645071508, 1298.910000000]: Odometry frame_id set to odom
[ INFO] [1545721821.646011383, 1298.910000000]: Publishing to tf is enabled
[ INFO] [1545721821.660871791, 1298.930000000]: Odometry params : wheel separation 0.43515, left wheel radius 0.193125, right wheel radius 0.193125
[ INFO] [1545721821.663344184, 1298.930000000]: Adding left wheel with joint name: left_wheel_joint and right wheel with joint name: right_wheel_joint
[ WARN] [1545721821.674875151, 1298.940000000]: updateConfig() called on a dynamic_reconfigure::Server that provides its own mutex. This can lead to deadlocks if updateConfig() is called during an update. Providing a mutex to the constructor is highly recommended in this case. Please forward this message to the node author.
[ INFO] [1545721821.678370805, 1298.940000000]: Dynamic Reconfigure:
DynamicParams:
Odometry parameters:
left wheel radius: 1
right wheel radius: 1
wheel separation: 1
Publication parameters:
Publish executed velocity command: 0
Publication rate: 50
Publish frame odom on tf: 1
[INFO] [1545721821.688834, 1298.960000]: Controller Spawner: Loaded controllers: joint_state_controller, diff_drive_controller
[INFO] [1545721821.698760, 1298.970000]: Started controllers: joint_state_controller, diff_drive_controller

$ roslaunch joy_control joy_control.launch

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ibert:35319/

SUMMARY
========

PARAMETERS
 * /joy/autorepeat_rate: 25
 * /joy/deadzone: 0.12
 * /joy/dev: /dev/input/js0
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /teleop_node/axis_angular: 0
 * /teleop_node/axis_linear: 1
 * /teleop_node/enable_button: 8
 * /teleop_node/enable_turbo_button: 10
 * /teleop_node/scale_angular: 0.4
 * /teleop_node/scale_linear: 0.7
 * /teleop_node/scale_linear_turbo: 1.5

NODES
  /
    joy (joy/joy_node)
    teleop_node (teleop_twist_joy/teleop_node)

ROS_MASTER_URI=http://localhost:11311

process[joy-1]: started with pid [9751]
process[teleop_node-2]: started with pid [9752]
[ INFO] [1545721858.912404584]: Teleop enable button 8.
[ INFO] [1545721858.912471209]: Turbo on button 10.
[ INFO] [1545721858.912499074]: Linear axis x on 1 at scale 0.700000.
[ INFO] [1545721858.912527219]: Turbo for linear axis x is scale 1.500000.
[ INFO] [1545721858.912556760]: Angular axis yaw on 0 at scale 0.400000.
[ INFO] [1545721858.912582600]: Turbo for angular axis yaw is scale 0.400000.
MoriKen254 commented 5 years ago

@hkato312 さん

メッセージのご開示、ありがとうございます。

確かに良さそうに見えますね。

よろしければ、rqt_graphの画像を公開いただけますか?念の為、具体的にどんなトピックが gazebo に入っているか確認できればと思います。

また、rostopic echo /cmd_vel で、実際に速度司令コマンドが流れているかも確認できればと思います。

hkato312 commented 5 years ago

rqt_graphでノードを描画したものを添付します。 rosgraph.pdf

$ rostopic echo /diff_drive_controller/cmd_vel

を実行してスティックを操作した場合、

---
linear:
  x: 0.0
  y: 0.0
  z: 0.0
 angular:
  x: 0.0
  y: 0.0
  z: 0.0
 ---

の値のまま変化はありませんでした。

しかし、PS ボタンまたは SELECT ボタンを押下すると、linear の x が -0.0 に、angular の z が -0.0 に変化しました。そこで、PS ボタンか SELECT ボタンのどちらかを押したまま左スティックを操作すると、linear の x と angular の z が様々な値に変化しました。この場合、シミュレーターのロボットも、左スティックの操作に応じて移動していることが確認できました。

PS ボタンか SELECT ボタンを離した途端に動かなくなりますので、押さなくても済む方法はありますでしょうか?

MoriKen254 commented 5 years ago

情報の御開示、誠にありがとうございます。大変参考となりました。

結論から申しますと、セレクトキーを押しながら操作できるという挙動が、正常の動作となります。

元々ROSで提供されているteleop_twist_joyのノードは、アナログスティックによるチャタリング等の誤動作防止を目的に、特定のボタンを押した状態で方向キーを入力した間だけ、速度指令をパブリッシュするのが一般的な仕様とされております。

そのデフォルトのキーID値が0で、ps3のコントローラの場合、たまたまそれがセレクトボタンとなってしまっています。

参考として、下記をご参照下さい。 http://wiki.ros.org/teleop_twist_joy

残念ながら、このノードの設定をパラメータレベルで編集するなら、セレクト以外のボタンをトリガにするよう変更することまでしかできません。

セレクトを押下せずに直接指令を与える場合、上記リポジトリをforkしてソースコードを編集する、あるいはスクラッチでからコーディングする、が解決策となります。

当サンプルコードにつきましては、教育用途と言う位置付けから、可能な限りROSで提供される汎用的なパッケージを利用するポリシーを取っておりますゆえ、ご了承頂けましたら幸いです。

よろしくお願い致します。

hkato312 commented 5 years ago

下記の"ボタン5"が、PS3コントローラのPS ボタンまたは SELECT ボタンに該当し、仕様ということで承知いたしました。詳細な情報ありがとうございました。

5.4.3 ノードの起動 ... ジョイパッド用ノードの起動 ... $ roslaunch turtlebot_teleop ps3_teleop.launch パソコンとジョイパッドの接続が確認できたら、ボタン5を押しながら左のスティックでロボットを操作できます。