LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@FAL19

DynamixelをROSで動かせるようにした話(3) トピック通信で動かしてみる編

Last updated at Posted at 2021-03-31

はじめに

前回の記事ではサービス通信でモータを一つづつ動かす方法を解説しました．
今回の記事ではトピック通信で動かす方法を説明します．

サブスクライバの詳細を確認してみる

ノードを作成する前に，どのようなトピックを送信すればよいのかソースコードを見て確認してみましょう．

以下のファイルを開いてみます．
・ヘッダファイル(dynamixel_workbench_controllers.h)

dynamixel-workbench/dynamixel_workbench_controllers/include/dynamixel_workbench_controllers/dynamixel_workbench_controllers.h

・ソースファイル(dynamixel_workbench_controllers.cpp)

dynamixel-workbench/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp

上記のファイル内に「trajectoryMsgCallback()」という関数があることが分かります．
これが/dynamixel_workbench/joint_trajectoryトピックに対するサブスクライバのコールバック関数です．
以下にその関数を抜粋したものを示します．

dynamixel_workbench_controllers.cpp
void DynamixelController::trajectoryMsgCallback(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
{
  uint8_t id_cnt = 0;
  bool result = false;
  WayPoint wp;

  if (is_moving_ == false){
    jnt_tra_msg_->joint_names.clear();
    jnt_tra_msg_->points.clear();
    pre_goal_.clear();

    result = getPresentPosition(msg->joint_names);
    if (result == false)
      ROS_ERROR("Failed to get Present Position");

    for (auto const& joint:msg->joint_names){
      ROS_INFO("'%s' is ready to move", joint.c_str());

      jnt_tra_msg_->joint_names.push_back(joint);
      id_cnt++;
    }

    if (id_cnt != 0){
      uint8_t cnt = 0;
      while(cnt < msg->points.size()){
        std::vector<WayPoint> goal;
        for (std::vector<int>::size_type id_num = 0; id_num < msg->points[cnt].positions.size(); id_num++){
          wp.position = msg->points[cnt].positions.at(id_num);

          if (msg->points[cnt].velocities.size() != 0)  wp.velocity = msg->points[cnt].velocities.at(id_num);
          else wp.velocity = 0.0f;

          if (msg->points[cnt].accelerations.size() != 0)  wp.acceleration = msg->points[cnt].accelerations.at(id_num);
          else wp.acceleration = 0.0f;

          goal.push_back(wp);
        }

        if (use_moveit_ == true){
          trajectory_msgs::JointTrajectoryPoint jnt_tra_point_msg;

          for (uint8_t id_num = 0; id_num < id_cnt; id_num++){
            jnt_tra_point_msg.positions.push_back(goal[id_num].position);
            jnt_tra_point_msg.velocities.push_back(goal[id_num].velocity);
            jnt_tra_point_msg.accelerations.push_back(goal[id_num].acceleration);
          }

          jnt_tra_msg_->points.push_back(jnt_tra_point_msg);

          cnt++;
        }
        else{
          jnt_tra_->setJointNum((uint8_t)msg->points[cnt].positions.size());

          double move_time = 0.0f;
          if (cnt == 0) move_time = msg->points[cnt].time_from_start.toSec();
          else move_time = msg->points[cnt].time_from_start.toSec() - msg->points[cnt-1].time_from_start.toSec();

          jnt_tra_->init(move_time, write_period_, pre_goal_, goal);

          std::vector<WayPoint> way_point;
          trajectory_msgs::JointTrajectoryPoint jnt_tra_point_msg;

          for (double index = 0.0; index < move_time; index = index + write_period_){
            way_point = jnt_tra_->getJointWayPoint(index);

            for (uint8_t id_num = 0; id_num < id_cnt; id_num++){
              jnt_tra_point_msg.positions.push_back(way_point[id_num].position);
              jnt_tra_point_msg.velocities.push_back(way_point[id_num].velocity);
              jnt_tra_point_msg.accelerations.push_back(way_point[id_num].acceleration);
            }

            jnt_tra_msg_->points.push_back(jnt_tra_point_msg);
            jnt_tra_point_msg.positions.clear();
            jnt_tra_point_msg.velocities.clear();
            jnt_tra_point_msg.accelerations.clear();
          }

          pre_goal_ = goal;
          cnt++;
        }
      }
      ROS_INFO("Succeeded to get joint trajectory!");
      is_moving_ = true;
    }
    else{
      ROS_WARN("Please check joint_name");
    }
  }
  else{
    ROS_INFO_THROTTLE(1, "Dynamixel is moving");
  }
}

上記の内容を見るに，
・トピック名：「/dynamixel_workbench/joint_trajectory」
・メッセージ型：「trajectory_msgs::JointTrajectory」
のメッセージを送れば良さそうですね．

ノードの作成

以上の内容を考慮して，Dynamixelを動かすノードを作成してみます．

dynamixel-workbench/dynamixel_workbench_controllers/srcにノードファイルを新規作成します．

// 長いので2つに分ける
$ cd ~/catkin_ws/src/my_dynamixel_pkgs/dynamixel-workbench/
$ cd dynamixel_workbench_controllers/src

$ touch my_trajectory_controller.cpp

作成したファイルを以下のように編集します．

dynamixel_workbench_controller/src/my_trajectory_controller.cpp
// my_trajectory_controller.cpp
// jointTrajectory型のmsgでdynamixelを動かすノード

# include <ros/ros.h>
# include <trajectory_msgs/JointTrajectory.h>

trajectory_msgs::JointTrajectory cmd;
ros::Publisher cmd_pub;

const int motor_num = 3;
const int point_num = 1;

int main(int argc, char** argv){
    ros::init(argc, argv, "my_trajectory_controller");
    ros::NodeHandle nh;
    ros::Rate loop_rate(100);

    cmd_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/dynamixel_workbench/joint_trajectory", 1);

    cmd.joint_names.resize(motor_num);
    cmd.points.resize(point_num);
    cmd.points[0].positions.resize(motor_num);
    cmd.points[1].positions.resize(motor_num);
    // cmd.points[2].positions.resize(motor_num);

    cmd.joint_names[0] = "Motor0";
    cmd.joint_names[1] = "Motor1";
    cmd.joint_names[2] = "Motor2";
    
    cmd.points[0].positions[0] = 0.0;    
    cmd.points[0].positions[1] = M_PI_2;
    cmd.points[0].positions[2] = 0.0;
    cmd.points[0].time_from_start = ros::Duration(1.0);

    while(ros::ok()){
        cmd.header.stamp = ros::Time::now();

        cmd_pub.publish(cmd);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

上記をビルドするために，CMakeLists.txtを編集して下記を追加します．

dynamixel_workbench_controller/CMakeLists.txt
# add this
add_executable(my_trajectory_controller src/my_trajectory_controller.cpp)
add_dependencies(my_trajectory_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(my_trajectory_controller ${catkin_LIBRARIES})

追加したら，catkin buildを実行しコンパイル．通ったら実行します．
（ノードを実行する前にdynamixelがROSに繋がっているか要確認！）

$ catkin build dynamixel_workbench_controllers
$ rosrun dynamixel_workbench_controllers my_trajectory_controller

問題点

しかしながら，この動かし方にも複数の問題点があると考えられます．

time_from_start変数について

上記のmy_trajectory_controller.cppを記述している際，てっきり変数の名前から
① 1秒後に全てのモータ角度が0[rad]に移動する．
② ①の2秒後（rosrunを実行してから3秒後）に全てのモータ角度が$\pi/2$[rad]に移動する
③ ①の2秒後（rosrunを実行してから5秒後）に全てのモータ角度が$\pi$[rad]に移動する

という動きを繰り返すと考えていたのですが，実際に動かしてみると（動画を見てみると）

① 1秒後に全てのモータ角度が0[rad]に移動する．
② ①の3秒後（rosrunを実行してから4秒後）に全てのモータ角度が$\pi/2$[rad]に移動する
③ ①の5秒後（rosrunを実行してから9秒後）に全てのモータ角度が$\pi$[rad]に移動する

という動きになってしまいました．

「timer_from_start」という名前から，
「なるほど，rosrunを実行してから何秒後にどこに動いてほしいのかを記述するんだな」
と勝手に思い込んでいましたが，どうやらそれは間違いだったようです．

異なる動作モードのモータを同時に動かしたい場合

今回は3つのモータ全てをExtended Position Control Modeで動かしましたが，場合によっては
・2つのモータを位置制御
・1つのモータを速度制御
で動かしたくなる場合もあると思います．

そのような場合，この動かし方だと位置制御でしか動かせないため，何らかの対策が必要になると考えられます．
（方法をご存知の方いらっしゃいましたら教えて下さい...）

任意の軌道に沿ってモータを動かせない

今回の場合だと
モータをある点からある点に動かす
といった制御は実現できるのですが，
モータを任意の軌道に追従させる
という制御を実現させるには厳しそうです．
（モータ角度をsin波状に角度を動かしたい場合など）

さいごに

今回の記事ではトピック通信でdynamixelを動かす方法を見てきましたが，どちらの方法もロボットを動かす際に使いにくいと感じました．
次回の記事ではdynamixel_workbench_controllers.cppを改良し，もっと手軽にかつ自由度の高い制御を行えるようにしたいと思います．

参考にしたサイト

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?