Dynamixel Workbenchを触ってみた #03


前回、「dynamixel_workbench_controllersを制御すれば、Dynamixelを自由自在に動かせるんじゃないか?」という仮説が出来たので、今回はdynamixel_workbench_controllersをいじってみたいと思います。

その前に、dynamixel_workbench_controllersがどうなってるのかちょっと気になったので、 dynamixel_workbench_controllersのソースからヘッダの参照がどうなっているのか、追ってみました。下記の通りです。


dynamixel_workbench_controllers.h
  |- dynamixel_workbench_toolbox/dynamixel_workbench.h
  |   |- dynamixel_driver.h
  |   |   |- dynamixel_tool.h
  |   |       |- dynamixel_item.h
  |   |
  |   |- dynamixel_sdk/dynamixel_sdk.h
  |
  |- dynamixel_workbench_msgs/DynamixelStateList.h
  |   |- DynamixelState.h
  |
  |- dynamixel_workbench_msgs/DynamixelCommand.h
  |   |- DynamixelCommandRequest.h
  |   |- DynamixelCommandResponse.h
  |
  |- trajectory_generator.h

なんと…。
単独で、起動不可だった、dynamixel_workbench_toolbox、dynamixel_sdk、dynamixel_workbench_msgsのパッケージが、全部インクルードされていますね。つまり、前々回に謎だったこれらは、やはりライブラリとして使われるモノということです。階層構造になっているのが面白いですね。

dynamixel_workbench_controllersに接続するノードを作ってみる(1)
Dynamixelから情報を読み出してみる

さて、いじり方としては、前回JointTrajectoryJointTrajectoryPointJointStateのtopicを理解しないといけなさそう…、という仮説が出来たので、まずは敷居の低そうな、JointStateをsubscribeするところから初めてみたいと思います。

いきなりですが、下記のようなノードを作ってみました。

// ~/catkin_ws/src/dynamixel_micchy/src/dynamixel_reader.cpp
#include "ros/ros.h"
#include "ros/time.h"

#include "std_msgs/Float64.h"
#include "std_msgs/String.h"

#include "sensor_msgs/JointState.h"

std_msgs::String joint1_name, joint2_name;
std_msgs::Float64 joint1_pos, joint2_pos;
std_msgs::Float64 joint1_vel, joint2_vel;
std_msgs::Float64 joint1_eff, joint2_eff;

void monitorJointState_callback(const sensor_msgs::JointState::ConstPtr& jointstate)
{
  joint1_name.data = jointstate->name[0];    // 名前読み出し
  joint2_name.data = jointstate->name[1];    // 名前読み出し

  joint1_pos.data = jointstate->position[0];    // ポジション読み出し    loop_rate.sleep();
  joint2_pos.data = jointstate->position[1];    // ポジション読み出し

  joint1_vel.data = jointstate->velocity[0];    // 速度読み出し
  joint2_vel.data = jointstate->velocity[1];    // 速度読み出し

  joint1_eff.data = jointstate->effort[0];    // 負荷読み出し
  joint2_eff.data = jointstate->effort[1];    // 負荷読み出し
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamixel_reader");  // ノードの初期化
  ros::NodeHandle nh; // ノードハンドラ  
  ros::Subscriber sub_joints;  // サブスクライバの作成
  sub_joints = nh.subscribe("/dynamixel_workbench/joint_states", 10, monitorJointState_callback);

  ros::Rate loop_rate(10);  // 制御周期10Hz設定

  while (ros::ok()) { // 無限ループ

    ros::spinOnce();   // コールバック関数を呼ぶ
    loop_rate.sleep();  //wait
    ROS_INFO("Joint1(%s)= %f | %f | %f ||| Joint2(%s)= %f | %f | %f ", joint1_name.data.c_str(),joint1_pos.data,joint1_vel.data,joint1_eff.data,joint2_name.data.c_str(),joint2_pos.data,joint2_vel.data,joint2_eff.data);
  }
  
  return 0;
}

これを立ち上げてみます。

1つめのターミナル
$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

2つめのターミナル
$ rosrun dynamixel_micchy dynamixel_reader

実行したところが、こんな感じです。

joint名、角度(radian)、速度、負荷、がばっちり取得できました。
rqt_graphは、下記の通りです。

さて、次回はサーボを回す方ですね!

コメント

  1. 田中信二 より:

    CMakeLists.txt や package.xml の内容をさしつかえなければ教えてください。

  2. カツハラ より:

    dynamixel_resder.cppをcatkin_makeしてみた所、15行目のmonitorJointState_callback関数でエラーが発生してしまいました。エラーしまいました。エラーコードは以下のとおりです。
    /home/user/catkin_ws/src/dynamixel_micchy/src/dynamixel_reader.cpp:15:76: error: expected ‘)’ before ‘;’ token
    void monitorJointState_callback(const sensor_msgs::JointState::ConstPtr& jointstate)
    ^
    セミコロン部をいくらか変更してみましたが、解決できませんでした。
    どのような問題があると考えられか、ご教授いただけないでしょうか。

    • みっちー より:

      返信が遅くなってすみません。
      こちらのパッケージはgithubで公開しております。
      https://github.com/dreamdrive/upperbody3

      該当するソースはこちらになります。
      https://github.com/dreamdrive/upperbody3/blob/master/upperbody3_control/src/dynamixel_reader.cpp

      解答になっていないかもしれませんが、とりあえず、パッケージまるごとcloneしていただいて、コンパイル通りますでしょうか?
      また、ROSのバージョンは、ros melodicになります。

      • カツハラ より:

        ありがとうございます。
        こちらも反応が遅くなってしまいすみません。
        その後、パッケージごと入れてみた所、無事に動きました。
        このページのプログラムだと、15行目のamp;の”;”の部分がエラーの原因になっていたようです。
        改めて、丁寧なご対応ありがとうございました。

        • みっちー より:

          なるほど、ブログにソースコードを貼るとエンコードの関係で「&」に余計な文字がくっついてしまうようです。
          こちらも気づかず、すみません。
          動いたようで良かったです。

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください

アーカイブ

広告

Top