コード解説（インターフェース)

follow_multi_dof_joint_trajectory_controller_handle.hpp
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
/**
 * @file follow_multi_dof_joint_trajectory_controller_handle.cpp
 * @brief Action client for ExecuteTrajectory action
 */

#ifndef MOVEIT_PLUGINS_FOLLOW_MULTI_DOF_TRAJECTORY_CONTROLLER_HANDLE
#define MOVEIT_PLUGINS_FOLLOW_MULTI_DOF_TRAJECTORY_CONTROLLER_HANDLE

#include <moveit_simple_controller_manager/action_based_controller_handle.h>
#include <moveit_msgs/ExecuteTrajectoryAction.h>

namespace dronedoc
{
/**
 * @brief Controller for multi DOF joint trajectory
 *
 * This is generally used for arms, but could also be used for multi-dof hands,
 * or anything using a control_mgs/FollowJointTrajectoryAction.
 */
class FollowMultiDOFJointTrajectoryControllerHandle
    : public moveit_simple_controller_manager::ActionBasedControllerHandle<moveit_msgs::ExecuteTrajectoryAction>
{
public:

  /**
   * @brief Constructor
   * @param name
   * @param action_ns
   */
  FollowMultiDOFJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns)
    : ActionBasedControllerHandle<moveit_msgs::ExecuteTrajectoryAction>(name, action_ns)
  {
  }

  /**
   * @brief Send ExecuteTrajectoryGoal message to action server
   * @param trajectory Trajectory to follow
   */
  bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override
  {
    ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController", "new trajectory to " << name_);

    if (!controller_action_client_)
      return false;

    if (!trajectory.joint_trajectory.points.empty())
    {
      ROS_WARN_NAMED("FollowMultiDOFJointTrajectoryController", "%s cannot execute trajectories(trajectory_msgs/JointTrajectory).", name_.c_str());
    }

    if (done_)
      ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController", "sending trajectory to " << name_);
    else
      ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController",
                             "sending continuation for the currently executed trajectory to " << name_);

    // Send ExecuteTrajectoryGoal message
    moveit_msgs::ExecuteTrajectoryGoal goal;
    goal.trajectory = trajectory;
    controller_action_client_->sendGoal(
        goal, boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
        boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerActiveCallback, this),
        boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));
    done_ = false;
    last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
    return true;
  }

  /**
   * @brief Cancel trajecotry execution
   */
  bool cancelExecution() override
  {
    // do whatever is needed to cancel execution
    return true;
  }

  /**
   * @brief Wait trajectory execution
   * @param duration
   */
  bool waitForExecution(const ros::Duration& duration) override
  {
    // wait for the current execution to finish
    return true;
  }

protected:

  /**
   * @brief Called when server complete action
   * @param state
   * @param result
   */
  void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
                              const moveit_msgs::ExecuteTrajectoryResultConstPtr& result)
  {
    // Output custom error message for FollowJointTrajectoryResult if necessary
    if (result)
    {
      if(result->error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
      {
        ROS_INFO_NAMED("FollowMultiDOFTrajectoryContoller", "Execution Succeeded.");
      }
      else
      {
        ROS_ERROR("Returned Error Code %d", result->error_code.val);
        ROS_ERROR("For Detailse of Error Code, see moveit_msgs/MoveItErrorCodes.msg");
      }
    }
    else
    {
      ROS_WARN_STREAM("Controller " << name_ << ": no result returned");
    }

    finishControllerExecution(state);
  }

  /**
   * @brief Called when goal become active
   */
  void controllerActiveCallback()
  {
    ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController", name_ << " started execution");
  }

  /**
   * @brief Called when feedback arrived from server
   * @param feedback
   */
  void controllerFeedbackCallback(const moveit_msgs::ExecuteTrajectoryFeedbackConstPtr& feedback)
  {
  }
};

}  // end namespace dronedoc

#endif // MOVEIT_PLUGINS_FOLLOW_MULTI_DOF_TRAJECTORY_CONTROLLER_HANDLE

このインターフェースの実装は FollowJointTrajectory を参考にしています。

#include <moveit_simple_controller_manager/action_based_controller_handle.h>
#include <moveit_msgs/ExecuteTrajectoryAction.h>

必要なヘッダファイルをインクルードします。 FollowMultiDOFJointTrajectoryインターフェースは、ExecuteTrajectory アクションを使用します。

namespace dronedoc

dronedoc 名前空間を使用します。

class FollowMultiDOFJointTrajectoryControllerHandle
    : public moveit_simple_controller_manager::ActionBasedControllerHandle<moveit_msgs::ExecuteTrajectoryAction>

ActionBasedControllerHandle を継承したクラスを定義します。

bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override

動作を実行する際には、MoveIt!に登録されたインターフェースの sendTrajectory 関数が、TrajectoryExecutionManagerによってコール されます。 このときに経路の情報（ moveit_msgs/RobotTrajectory メッセージ）が与えられるので、これをアクションのゴールとして送信します。

if (!trajectory.joint_trajectory.points.empty())
{
    ROS_WARN_NAMED("FollowMultiDOFJointTrajectoryController", "%s cannot execute trajectories(trajectory_msgs/JointTrajectory).", name_.c_str());
}

joint_trajectory フィールドに経由点が格納されている場合は、実行されない旨の警告を表示します。

moveit_msgs::ExecuteTrajectoryGoal goal;
goal.trajectory = trajectory;

アクションサーバに送信するためのメッセージを作成します。

controller_action_client_->sendGoal(
    goal, boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
    boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerActiveCallback, this),
    boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));

アクションサーバにゴールを送信します。 同時に、種々のコールバック関数を登録します。

void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
                            const moveit_msgs::ExecuteTrajectoryResultConstPtr& result)
{
    // Output custom error message for FollowJointTrajectoryResult if necessary
    if (result)
    {
        if(result->error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
        {
            ROS_INFO_NAMED("FollowMultiDOFTrajectoryContoller", "Execution Succeeded.");
        }
        else
        {
            ROS_ERROR("Returned Error Code %d", result->error_code.val);
            ROS_ERROR("For Detailse of Error Code, see moveit_msgs/MoveItErrorCodes.msg");
        }
    }
    else
    {
        ROS_WARN_STREAM("Controller " << name_ << ": no result returned");
    }

    finishControllerExecution(state);
}

アクションサーバが実行を終了した場合に呼ばれます。 エラーが発生した場合はエラーメッセージを、成功したら成功したことを通知するメッセージを表示します。