Skip to content

Commit

Permalink
checking the existance all joints in required group on HrpsysJointTra…
Browse files Browse the repository at this point in the history 
…jectoryBridge (ignore not existing joint)
  • Loading branch information
[email protected] committed Aug 29, 2013
1 parent 239cacb commit 7f8f779
Showing 1 changed file with 13 additions and 0 deletions.
13 changes: 13 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -275,6 +275,19 @@ onJointTrajectory(trajectory_msgs::JointTrajectory trajectory) {
duration.length(trajectory.points.size()) ;

std::vector<std::string> joint_names = trajectory.joint_names;
if (joint_names.size() < joint_list.size()) {

This comment has been minimized.

Sign in to view

Copy link
@k-okada

k-okada Feb 14, 2017

Member

@YoheiKakiuchi
ここのエラーチェックなんだけど、
joint_names = 指令値の関節数．
joint_list = モデルの関節数（あるいはパラメータで設定した関節数）

どういう不等号にしたらいいかわからないんだけど、#996
で以下のようにエラーになっていて、joint_namesはJ1-J7なんだけど、モデルから取ってくるとHANDもある．
このテストはPA10をつかっていて、limbではなくてfullbodyなので、このPRでfullbodyもSeqState... をつかわずに、
HrpsysJoint .. を使うようにしたのが原因で、いままでSeqStateではエラーチェックはしていなかった．
とりあえず、想定していない関節を指令されたら、なんか変、ということで、エラーにするほうこうにしたのと、

その後ろでも、指令値に知らない関節があったらエラーだけど、必要な関節が全て指令値になくてもOKにしてみた．
b18d13d どうでしょうか．

[rtmlaunch] Activate <- Inactive /localhost:2809/ForwardKinematicsServiceROSBridge.rtc
[ WARN] [1487065803.194794532]: HrpsysSeqStateROSBridge0 Connected to SequencePlayer (seq) version 315.10.1
[ERROR] [1487065803.194859921]:  -> J1 1 0
[ERROR] [1487065803.194879924]:  -> J2 2 1
[ERROR] [1487065803.194890758]:  -> J3 3 2
[ERROR] [1487065803.194904312]:  -> J4 4 3
[ERROR] [1487065803.194912656]:  -> J5 5 4
[ERROR] [1487065803.194922645]:  -> J6 6 5
[ERROR] [1487065803.194931475]:  -> J7 7 6
[ERROR] [1487065803.194940439]:  -> HAND_L 8 7
[ERROR] [1487065803.194953082]:  -> HAND_R 9 8
[rtmlaunch] Activate <- Inactive /localhost:2809/StateHolderServiceROSBridge.rtc
[rtmlaunch] Activate <- Inactive /localhost:2809/CollisionDetectorServiceROSBridge.rtc
[rtmlaunch] Activate <- Inactive /localhost:2809/SoftErrorLimiterServiceROSBridge.rtc
[addJointGroup] group name = fullbody
[ WARN] [1487065803.255335646]: param: /controller_configuration, param does not exist.
[ INFO] [1487065803.256990604, 3.639999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 0[Hz] (exec 1 Hz, dropped 0)
[ INFO] [1487065804.260645001, 4.684999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 209[Hz] (exec 1582 Hz, dropped 1)
[INFO] [WallTime: 1487065804.600855] [5.025000] check 0 collision status, 0.000000 Hz
[ INFO] [1487065805.263326348, 5.689999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1505 Hz, dropped 0)
[ INFO] [1487065806.265165164, 6.694999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1524 Hz, dropped 0)
[co] CAUTION!! The robot is moving without checking self collision detection!!! please define collision_pair in configuration file
[ INFO] [1487065807.272725803, 7.699999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1434 Hz, dropped 0)
[ INFO] [1487065808.274229109, 8.704999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 202[Hz] (exec 1346 Hz, dropped 0)
[ INFO] [1487065809.276325365, 9.715000000]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1289 Hz, dropped 0)
[ INFO] [1487065809.557919228, 9.990000000]: [HrpsysSeqStateROSBridge0] @onJointTrajectoryActionGoal
[ INFO] [1487065809.558222669, 9.990000000]: [HrpsysSeqStateROSBridge0] @onJointTrajectoryAction (fullbody)
[ERROR] [1487065809.558411242, 9.990000000]: joint list -> 0 J1
[ERROR] [1487065809.558572759, 9.990000000]: joint list -> 1 J2
[ERROR] [1487065809.558732735, 9.990000000]: joint list -> 2 J3
[ERROR] [1487065809.558883549, 9.990000000]: joint list -> 3 J4
[ERROR] [1487065809.559029561, 9.990000000]: joint list -> 4 J5
[ERROR] [1487065809.559184975, 9.990000000]: joint list -> 5 J6
[ERROR] [1487065809.559499950, 9.990000000]: joint list -> 6 J7
[ERROR] [1487065809.559729558, 9.990000000]: joint list -> 7 HAND_L
[ERROR] [1487065809.559878749, 9.990000000]: joint list -> 8 HAND_R
[ERROR] [1487065809.560032021, 9.990000000]: joint names -> 0 J1
[ERROR] [1487065809.560457834, 9.990000000]: joint names -> 1 J2
[ERROR] [1487065809.560635505, 9.990000000]: joint names -> 2 J3
[ERROR] [1487065809.560741246, 9.990000000]: joint names -> 3 J4
[ERROR] [1487065809.560837790, 9.990000000]: joint names -> 4 J5
[ERROR] [1487065809.560933222, 9.990000000]: joint names -> 5 J6
[ERROR] [1487065809.561053964, 9.990000000]: joint names -> 6 J7
[ERROR] [1487065809.561163689, 9.990000000]: [HrpsysSeqStateROSBridge0] @onJointTrajectoryAction / Error : required joint_names.size() = 7 < joint_list.size() = 9
[co] CAUTION!! The robot is moving without checking self collision detection!!! please define collision_pair in configuration file
[rtmlaunch] check connection/activation
ROS_ERROR_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction / Error : "
<< "required joint_names.size() = " << joint_names.size()
<< " < joint_list.size() = " << joint_list.size() );
return;
}
for (unsigned int i = 0; i < joint_list.size(); i++) {
if (count(joint_names.begin(), joint_names.end(), joint_list[i]) != 1) {
ROS_ERROR_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction / Error : "
<< "joint : " << joint_list[i] << " did not exist in the required trajectory.");
return;
}
}

ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : trajectory.points.size() " << trajectory.points.size());
for (unsigned int i = 0; i < trajectory.points.size(); i++) {
Expand Down

2 comments on commit 7f8f779

@YoheiKakiuchi
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

はい、それがいいように思います。
joint_names > joint_list : 使われない指令値がある 意図したのに動かない関節があるが、問題は起こらない
joint_names < joint_list : 不定になる指令値がある Seqに不定値や0などの意図しない指令値を入れることになって、ロボットが意図せず動くので問題
ということですね。

joint_names != joint_list の場合は、warningを出すというのでどうでしょうか？
常にwarnigが出ると目障りということもあるかもしれませんが。

@k-okada
Copy link
Member

@k-okada k-okada commented on 7f8f779 Feb 14, 2017 via email

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please sign in to comment.