コード解説(インターフェース)¶
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);
}
アクションサーバが実行を終了した場合に呼ばれます。 エラーが発生した場合はエラーメッセージを、成功したら成功したことを通知するメッセージを表示します。