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から情報を読み出してみる
さて、いじり方としては、前回、JointTrajectory、JointTrajectoryPoint、JointStateの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は、下記の通りです。
さて、次回はサーボを回す方ですね！
CMakeLists.txt や package.xml の内容をさしつかえなければ教えてください。
コメントありがとうございます。
確かにソースコードだけでは実行できませんね。
githubにまとめる時間がなかったので、取り急ぎ下記に該当ファイルだけアップしました。
https://dream-drive.net/temp/package.xml
https://dream-drive.net/temp/CMakeLists.txt
#03 #04の3つのノード（dynamixel2dynamixel、dynamixel_reader、dynamixel_writer）を含んでいます。
よろしくお願いします。
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;の”;”の部分がエラーの原因になっていたようです。
改めて、丁寧なご対応ありがとうございました。
なるほど、ブログにソースコードを貼るとエンコードの関係で「＆」に余計な文字がくっついてしまうようです。
こちらも気づかず、すみません。
動いたようで良かったです。