moveitを使います!リアルアームの制御(5)-リアルアームノードのactionサービスプログラムの作成
9622 ワード
前回の記事では、ROSのactionの基本的な知識を簡単に紹介しました.実際のアームを制御するために、moveitプロファイルを変更し、movetiが起動した後にcontrol_を備えました.msgs::FollowJointTrajectoryタイプのactionクライアントです.次に、このタイプのactionの残りの半分を完了し、actionのサービス側を実現するためにC++プログラムを作成します.下部ロボット制御プログラムは一般的に大きな違いがあるため,ここではプログラミングのポイントだけを示した.
予備条件:
1.アーム底部駆動が完了しました.あなたのアームが大手会社から買ってきたにしても、自分で作ったにしても、下の駆動制御部分を事前に完成してください.ここでは、あなたの駆動制御プログラムにはROS関連コードが含まれていないと仮定し、メッセージがなく、actionがないなどのROSならではのものがあります.アームの基本的な動きをC++言語で制御するだけです(たとえば、関節角を指定したり、アームの末端を指定した位置と姿勢に移動したりします).これは重要です.moveitを使用して実際のアームを制御することを目標としているので、下部駆動に問題があれば、後続の内容は話せません.
2.アームは路点(軌跡)に従って動くことができる.これは一般的なアームの基本的な機能であるはずです.ここでの軌跡は1つの路点からなり,路点はアームが空間を移動する際に通過しなければならない点と理解され,1つの路点が直列に並んで軌跡を形成する.
路点のデータ構造は一般的に6自由度アームのような関節角度のセットであり、6つの独立した関節があり、アームが空間の唯一の位置にあることを決定すると、(joint 1,joint 2,joint 3,joint 4,joint 5,joint 6)で1つの路点を表すことができる.なぜアームが路点に従って動くように要求されたのですか?moveitが最終的に発行した制御指令の実質は一連の路点であるため、アームがこの一連の路点を実行できる限り、アームが計画された軌跡に従って運動する目的を達成します(アームが路点を実行する機能を備えていない場合は、moveitが送信したデータを自分で変換する必要があるかもしれません).
アーム制御命令を次の2つの関数として抽象化してください.関数の名前はどうでもいいです.以下の機能さえあればいいです.
関数1は、実行するすべてのパスをコンテナに存在させると仮定して、パスポイントを追加します.関数2は、この「コンテナ」の路点を実行するために使用されます.すなわち、一定の規則に従って差分値を行い、アームを軌跡に沿って動かす空間軌跡を形成します.
以下の多くのコードはこのような抽象に対して,単純な複製では通用せず,理解して自分のプログラムを類似の構造に改造してこそよい.
UR、Auboのような商品化されたアームを使用している場合は、上記の予備条件は天然に満たされており、これらの機能メーカーは対応するライブラリ関数に統合されており、ユーザーが具体的な実現を心配する必要はありません.これらのライブラリ関数にどのようなデータを伝えるかに関心を持つ必要があります.
具体的な改造過程:
1、ROSでドライバをカプセル化します.ROSでカプセル化するということは、あなたの普通のC++プログラムをROSノードプログラムに改造し、カプセル化は比較的こまごましていますが、実は簡単です.この部分はROSチュートリアルbeginner部分を参照することができます.もともとあなたが実行していたのはC++ドライバで、今このファイルにはROS成分が入っていて、rosノードになっています.簡単です.
このとき、アームドライバはROSノードとして実行できるはずです.今、このノードが購読したり発表したりしていないとしても、確かにROSノードです.次が主な内容です.
2、actionサービス側コード(1)cmakelistを加える.txtとpackage.xmlにactionを加える依存度は以下cmakelistである.txtファイル
以下はpackageです.xmlファイル(formate 2フォーマット)
(2)actionサービスヘッダファイルの追加
(3)サービス側オブジェクトの定義
注意コントロール_msgs::FollowJointTrajectoryというタイプです.このタイプは前の記事で述べたように、ROSが持参したアーム関節角に対するactionタイプであり、moveitはactionクライアントとしてもこのタイプであるため、ここで記述したサービス側はクラス型を一致させなければなりません.
(4)このサービス側オブジェクトを初期化まず公式サイトがactionサービス側オブジェクトをどのように初期化したかを見てみましょう.
公式サイトではactionの実装は1つのクラスで、as_このクラスのメンバー変数としてas_の初期化は、サービス側オブジェクトas_の初期化はクラスより先に行われるので、as_の初期化書き込み位置はエラーを報告します.
あなたもクラスに基づいてactionを実装していると仮定し、このクラスの名前をAuboDriverと仮定すると、AuboDriverの構造関数の基本形式は次のようになります.
よく比較すると、この書き方は公式サイトのサンプルプログラムの書き方とほぼ一致しています.as_のコンストラクション関数パラメータの基本的な意味は、nh:actionが存在するrosノードのノードハンドル名です.
「aubo_i 5/follow_joint_trajectory」:これはactionの名前であり、actionサービス側がクライアントとペアを組む根拠であり、前の記事で述べたcontrollers.yamlに書かれているパラメータnameとaction_nsはクライアントactionの名前を共に決定するので、サービス側の名前はcontrollersを参照しなければならない.yamlのパラメータを書いてこそペアになります.
boost::bind(&AuboDriver::goalCB,this,_1):boostこれは固定用法で、深く研究しないで、それはAuboDriver::goalCBという関数を縛って、AuboDriverはas_です属するクラスの名前、goalCB()はその中の1つのメンバー関数で、この関数はas_に書かれています.コンストラクション関数の3番目のパラメータビットは、actionターゲットを受け入れるコールバック関数(関数名は自分で変更可能)であることを示し、actionクライアントが動作を要求すると、この動作はターゲットとみなされ、actionサービス側に伝達される.このときmoveitのactionクライアントにとって、このターゲットはアームの動き軌跡であり、サービス側がターゲットを受信すると、goalCBという関数が自動的にコールバックされ、ターゲット(軌跡)がパラメータとして渡されます.
(5)コールバック関数を書きます.前のステップでgoalCB()というコールバック関数が現れました.では、次はこのコールバック関数を具体的に作成します.これは実際のアームが動く重要な一環です.
上記のプログラムの例には、2つの疑問点が表示されています.
疑問点1:なぜ形参をコピーするのですか?ROS下のコールバック関数パラメータの書き方は多く見られるはずですが、constと書くのですが・・・という形式で、これもC++パラメータが推奨する形式(定数ポインタ)で、パラメータのコピーによる余分なオーバーヘッドを回避することを目的としていますが、ここでは、関数にパラメータを渡す際にconst定数ポインタを使用する形式はありません.このようにパラメータを直接渡すのは実際にパラメータをコピーすることであり、これは明らかにコピーのオーバーヘッドをもたらします.なぜそうするのでしょうか.このパラメータを変更する必要がありますが、パラメータが指すターゲットオブジェクトを直接変更することはできません.そうすると、元のデータが破壊され、プログラミングでは推奨されません.ここでコピーをコピーします.目的は、元のデータを破壊することなく、このデータを変更することです.
疑問点2:reorder_についてtraj_jointsという関数の役割は何ですか.この関数はユーザーが自分で書く必要があります.役割は、前の疑問点で述べた形参のコピーを修正し、よく形参を伝達することです.なぜ修正しますか.内部データの順序が間違っているからです!
私たちはgoalという変数でactionクライアントから送られてきたターゲットを格納しました.このターゲットは何ですか.コントロールタイプですmsgs::道路ポイントのセットを表すFollowJointTrajectoryアクションのアクションタイプ.何百ものルートがあるかもしれませんが、各ルートには関節角のセットが記録されています(6自由度のアームについてはjoint 1-joint 6です).しかし、そのデータ構造は決して簡単ではありません.詳細な構造はcontrolを参照してください.msgs::FollowJointTrajectoryの定義では、各関節角に対応する名前があり、これらの関節名は最初のアームxacroモデル、および後のcontrollersである.yamlの関節の名前は脈々と受け継がれています.ルートの関節角を取得したい場合は、goal.trajectory.points[i]だが、コーナーマークiはi番目の関節ではなくgoalに対応する.trajectory.joint_names[i]が指す関節なので、まず関節の名前を見つけてから、対応する関節の角を探さなければなりません.そうしないと、張冠李戴のことが起こりかねません.ここではreorder_を書くことをお勧めします.traj_joints()関数は、元のデータを再マッピングしてソートすることで、下付きスケールを使用して関節角を読み取ると、対応する角度が私たちが想定している1-6関節に一つ一つ対応し、自分の手段でパラメータのデータを実際の関節角に対応させることができます.ここでは、この現象を強調したいだけです.関節角が関節名に対応しているというポイントを無視すると、アームの動きが予想外の結果になる可能性があります.軽いとアラームが動かず、重いとアームが周囲の人や物に当たってしまうので、スクリーン印刷など多くのデバッグ手段でデータの順番が正しいかどうかを詳しく検証してください.
残りのコードは、特定のアームの駆動に関連する実行関数であり、実行に成功した後、前に定義したgoal_handle_実行に成功したメッセージをフィードバックし、actionクライアントに送信したトラックリクエストが正常に実行されたことを伝えます.
整理して、本編はactionサービス端の主な構想を編纂します:1、actionの依存項目を追加して、actionオブジェクトを定義して、この関系をコンパイルすることを確保します;2、actionのコールバック関数を実現し、伝達されたデータを正しく読み取り、情報伝達という関門を確保する.3、ユーザー自身の駆動によってactionの目標を実行し、この関門を実行することを確保する.
これにより、moveitを用いて実際のアームの動きを制御することが基本的に実現される.
予備条件:
1.アーム底部駆動が完了しました.あなたのアームが大手会社から買ってきたにしても、自分で作ったにしても、下の駆動制御部分を事前に完成してください.ここでは、あなたの駆動制御プログラムにはROS関連コードが含まれていないと仮定し、メッセージがなく、actionがないなどのROSならではのものがあります.アームの基本的な動きをC++言語で制御するだけです(たとえば、関節角を指定したり、アームの末端を指定した位置と姿勢に移動したりします).これは重要です.moveitを使用して実際のアームを制御することを目標としているので、下部駆動に問題があれば、後続の内容は話せません.
2.アームは路点(軌跡)に従って動くことができる.これは一般的なアームの基本的な機能であるはずです.ここでの軌跡は1つの路点からなり,路点はアームが空間を移動する際に通過しなければならない点と理解され,1つの路点が直列に並んで軌跡を形成する.
路点のデータ構造は一般的に6自由度アームのような関節角度のセットであり、6つの独立した関節があり、アームが空間の唯一の位置にあることを決定すると、(joint 1,joint 2,joint 3,joint 4,joint 5,joint 6)で1つの路点を表すことができる.なぜアームが路点に従って動くように要求されたのですか?moveitが最終的に発行した制御指令の実質は一連の路点であるため、アームがこの一連の路点を実行できる限り、アームが計画された軌跡に従って運動する目的を達成します(アームが路点を実行する機能を備えていない場合は、moveitが送信したデータを自分で変換する必要があるかもしれません).
アーム制御命令を次の2つの関数として抽象化してください.関数の名前はどうでもいいです.以下の機能さえあればいいです.
1： add_wayPoint();
2：trackMove();関数1は、実行するすべてのパスをコンテナに存在させると仮定して、パスポイントを追加します.関数2は、この「コンテナ」の路点を実行するために使用されます.すなわち、一定の規則に従って差分値を行い、アームを軌跡に沿って動かす空間軌跡を形成します.
以下の多くのコードはこのような抽象に対して,単純な複製では通用せず,理解して自分のプログラムを類似の構造に改造してこそよい.
UR、Auboのような商品化されたアームを使用している場合は、上記の予備条件は天然に満たされており、これらの機能メーカーは対応するライブラリ関数に統合されており、ユーザーが具体的な実現を心配する必要はありません.これらのライブラリ関数にどのようなデータを伝えるかに関心を持つ必要があります.
具体的な改造過程:
1、ROSでドライバをカプセル化します.ROSでカプセル化するということは、あなたの普通のC++プログラムをROSノードプログラムに改造し、カプセル化は比較的こまごましていますが、実は簡単です.この部分はROSチュートリアルbeginner部分を参照することができます.もともとあなたが実行していたのはC++ドライバで、今このファイルにはROS成分が入っていて、rosノードになっています.簡単です.
このとき、アームドライバはROSノードとして実行できるはずです.今、このノードが購読したり発表したりしていないとしても、確かにROSノードです.次が主な内容です.
2、actionサービス側コード(1)cmakelistを加える.txtとpackage.xmlにactionを加える依存度は以下cmakelistである.txtファイル
find_package(catkin REQUIRED COMPONENTS
……
# action ， action
actionlib
#action ， ， moveit
moveit_msgs
……
)catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS …… actionlib moveit_msgs ……
)以下はpackageです.xmlファイル(formate 2フォーマット)
<build_depend>actionlibbuild_depend>
<build_depend>moveit_msgsbuild_depend><exec_depend>actionlibexec_depend>
<exec_depend>moveit_msgsexec_depend>(2)actionサービスヘッダファイルの追加
//action ，
#include "actionlib/server/action_server.h"
//action ， ， action
#include "actionlib/server/server_goal_handle.h"
(3)サービス側オブジェクトの定義
// action
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
// action
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
// action ，
control_msgs::FollowJointTrajectoryResult result_;
注意コントロール_msgs::FollowJointTrajectoryというタイプです.このタイプは前の記事で述べたように、ROSが持参したアーム関節角に対するactionタイプであり、moveitはactionクライアントとしてもこのタイプであるため、ここで記述したサービス側はクラス型を一致させなければなりません.
(4)このサービス側オブジェクトを初期化まず公式サイトがactionサービス側オブジェクトをどのように初期化したかを見てみましょう.
公式サイトではactionの実装は1つのクラスで、as_このクラスのメンバー変数としてas_の初期化は、サービス側オブジェクトas_の初期化はクラスより先に行われるので、as_の初期化書き込み位置はエラーを報告します.
あなたもクラスに基づいてactionを実装していると仮定し、このクラスの名前をAuboDriverと仮定すると、AuboDriverの構造関数の基本形式は次のようになります.
AuboDriver::AuboDriver():
as_(nh, "aubo_i5/follow_joint_trajectory",
boost::bind(&AuboDriver::goalCB, this, _1),
false)
{
as_.start(); //action
}よく比較すると、この書き方は公式サイトのサンプルプログラムの書き方とほぼ一致しています.as_のコンストラクション関数パラメータの基本的な意味は、nh:actionが存在するrosノードのノードハンドル名です.
「aubo_i 5/follow_joint_trajectory」:これはactionの名前であり、actionサービス側がクライアントとペアを組む根拠であり、前の記事で述べたcontrollers.yamlに書かれているパラメータnameとaction_nsはクライアントactionの名前を共に決定するので、サービス側の名前はcontrollersを参照しなければならない.yamlのパラメータを書いてこそペアになります.
boost::bind(&AuboDriver::goalCB,this,_1):boostこれは固定用法で、深く研究しないで、それはAuboDriver::goalCBという関数を縛って、AuboDriverはas_です属するクラスの名前、goalCB()はその中の1つのメンバー関数で、この関数はas_に書かれています.コンストラクション関数の3番目のパラメータビットは、actionターゲットを受け入れるコールバック関数(関数名は自分で変更可能)であることを示し、actionクライアントが動作を要求すると、この動作はターゲットとみなされ、actionサービス側に伝達される.このときmoveitのactionクライアントにとって、このターゲットはアームの動き軌跡であり、サービス側がターゲットを受信すると、goalCBという関数が自動的にコールバックされ、ターゲット(軌跡)がパラメータとして渡されます.
(5)コールバック関数を書きます.前のステップでgoalCB()というコールバック関数が現れました.では、次はこのコールバック関数を具体的に作成します.これは実際のアームが動く重要な一環です.
void AuboDriver::goalCB( actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{
// 1： ？
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal(); //make a copy that we can modify
// action ，
goal_handle_ = gh;
// 2： ？
reorder_traj_joints(goal.trajectory);
// ，
Add_waypoint(goal.trajectory);
// ，
if（TrackMove()） // 1， 0
{
//
goal_handle_.setAccepted();
result_.error_code = result_.SUCCESSFUL;
goal_handle_.setSucceeded(result_);
}
}上記のプログラムの例には、2つの疑問点が表示されています.
疑問点1:なぜ形参をコピーするのですか?ROS下のコールバック関数パラメータの書き方は多く見られるはずですが、constと書くのですが・・・という形式で、これもC++パラメータが推奨する形式(定数ポインタ)で、パラメータのコピーによる余分なオーバーヘッドを回避することを目的としていますが、ここでは、関数にパラメータを渡す際にconst定数ポインタを使用する形式はありません.このようにパラメータを直接渡すのは実際にパラメータをコピーすることであり、これは明らかにコピーのオーバーヘッドをもたらします.なぜそうするのでしょうか.このパラメータを変更する必要がありますが、パラメータが指すターゲットオブジェクトを直接変更することはできません.そうすると、元のデータが破壊され、プログラミングでは推奨されません.ここでコピーをコピーします.目的は、元のデータを破壊することなく、このデータを変更することです.
疑問点2:reorder_についてtraj_jointsという関数の役割は何ですか.この関数はユーザーが自分で書く必要があります.役割は、前の疑問点で述べた形参のコピーを修正し、よく形参を伝達することです.なぜ修正しますか.内部データの順序が間違っているからです!
私たちはgoalという変数でactionクライアントから送られてきたターゲットを格納しました.このターゲットは何ですか.コントロールタイプですmsgs::道路ポイントのセットを表すFollowJointTrajectoryアクションのアクションタイプ.何百ものルートがあるかもしれませんが、各ルートには関節角のセットが記録されています(6自由度のアームについてはjoint 1-joint 6です).しかし、そのデータ構造は決して簡単ではありません.詳細な構造はcontrolを参照してください.msgs::FollowJointTrajectoryの定義では、各関節角に対応する名前があり、これらの関節名は最初のアームxacroモデル、および後のcontrollersである.yamlの関節の名前は脈々と受け継がれています.ルートの関節角を取得したい場合は、goal.trajectory.points[i]だが、コーナーマークiはi番目の関節ではなくgoalに対応する.trajectory.joint_names[i]が指す関節なので、まず関節の名前を見つけてから、対応する関節の角を探さなければなりません.そうしないと、張冠李戴のことが起こりかねません.ここではreorder_を書くことをお勧めします.traj_joints()関数は、元のデータを再マッピングしてソートすることで、下付きスケールを使用して関節角を読み取ると、対応する角度が私たちが想定している1-6関節に一つ一つ対応し、自分の手段でパラメータのデータを実際の関節角に対応させることができます.ここでは、この現象を強調したいだけです.関節角が関節名に対応しているというポイントを無視すると、アームの動きが予想外の結果になる可能性があります.軽いとアラームが動かず、重いとアームが周囲の人や物に当たってしまうので、スクリーン印刷など多くのデバッグ手段でデータの順番が正しいかどうかを詳しく検証してください.
残りのコードは、特定のアームの駆動に関連する実行関数であり、実行に成功した後、前に定義したgoal_handle_実行に成功したメッセージをフィードバックし、actionクライアントに送信したトラックリクエストが正常に実行されたことを伝えます.
整理して、本編はactionサービス端の主な構想を編纂します:1、actionの依存項目を追加して、actionオブジェクトを定義して、この関系をコンパイルすることを確保します;2、actionのコールバック関数を実現し、伝達されたデータを正しく読み取り、情報伝達という関門を確保する.3、ユーザー自身の駆動によってactionの目標を実行し、この関門を実行することを確保する.
これにより、moveitを用いて実際のアームの動きを制御することが基本的に実現される.