{"action":"created","comment":{"url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/comments/92585882","pull_request_review_id":13103401,"id":92585882,"diff_hunk":"@@ -48,14 +52,135 @@ Item* WorldRosItem::doDuplicate() const\n   return new WorldRosItem(*this);\n }\n \n+#define copy_wrenches_data(dylink, dst, wrench_index, total_force, total_torque) \\\n+  do {                                                                           \\\n+    DyLink::ConstraintForceArray& cfa = dylink->constraintForces();              \\\n+                                                                                 \\\n+    for (size_t cfa_idx = 0; cfa_idx < cfa.size(); cfa_idx++) {                  \\\n+      dst->wrenches[wrench_index].force.x  = cfa[cfa_idx].force[0];              \\\n+      dst->wrenches[wrench_index].force.y  = cfa[cfa_idx].force[1];              \\\n+      dst->wrenches[wrench_index].force.z  = cfa[cfa_idx].force[2];              \\\n+      dst->wrenches[wrench_index].torque.x = cfa[cfa_idx].point[0];              \\\n+      dst->wrenches[wrench_index].torque.y = cfa[cfa_idx].point[1];              \\\n+      dst->wrenches[wrench_index].torque.z = cfa[cfa_idx].point[2];              \\\n+      wrench_index++;                                                            \\\n+    }                                                                            \\\n+                                                                                 \\\n+    total_force  += dylink->f_ext();                                             \\\n+    total_torque += dylink->tau_ext();                                           \\\n+  } while(0)\n+\n+void WorldRosItem::publishContactsState()\n+{\n+  if (! sim_access_ || ! sim_access_->get_collisions()) {\n+    return;\n+  }\n+\n+  gazebo_msgs::ContactsState cs;\n+  rosgraph_msgs::Clock       tm;\n+  std::string                info;\n+  CollisionLinkPairListPtr   collision_pairs;\n+  size_t                     i;\n+\n+  tm.clock.fromSec(sim->currentTime());\n+\n+  info                 = \"world: \\\"\" + world->name() + \"\\\"\";\n+  cs.header.stamp.sec  = tm.clock.sec;\n+  cs.header.stamp.nsec = tm.clock.nsec;\n+  collision_pairs      = sim_access_->get_collisions();\n+  i                    = 0;\n+\n+  cs.states.resize(collision_pairs->size());\n+\n+  for (CollisionLinkPairList::iterator it = collision_pairs->begin(); it != collision_pairs->end(); it++) {\n+    gazebo_msgs::ContactState* dst     = &(cs.states[i++]);\n+    CollisionLinkPairPtr       p       = *it;\n+    size_t                     cols_sz = p->collisions.size();\n+\n+    /*\n+      Copy results of link pairs name.\n+     */\n+    dst->info            = info;\n+    dst->collision1_name = p->body[0]->name() + \"::\" + p->link[0]->name();\n+    dst->collision2_name = p->body[1]->name() + \"::\" + p->link[1]->name();\n+\n+    /*\n+      Copy results of force and torque.\n+     */\n+    DyLink* dylink0;\n+    DyLink* dylink1;\n+    size_t  wrch_sz;\n+\n+    dylink0 = dynamic_cast<DyLink*>(p->link[0]);\n+    dylink1 = dynamic_cast<DyLink*>(p->link[1]);\n+    wrch_sz = 0;\n+\n+    if (dylink0) {\n+      wrch_sz += dylink0->constraintForces().size();\n+    }\n+\n+    if (dylink1) {\n+      wrch_sz += dylink1->constraintForces().size();\n+    }\n+\n+    dst->wrenches.resize(wrch_sz);\n+\n+    if (wrch_sz > 0) {\n+      Vector3 total_force(Vector3::Zero());\n+      Vector3 total_torque(Vector3::Zero());\n+      size_t  wrch_idx = 0;\n+\n+      if (dylink0) {\n+        copy_wrenches_data(dylink0, dst, wrch_idx, total_force, total_torque);\n+      }\n+\n+      if (dylink1) {\n+        copy_wrenches_data(dylink1, dst, wrch_idx, total_force, total_torque);","path":"choreonoid_plugins/src/WorldRosItem.cpp","position":125,"original_position":125,"commit_id":"9f123d949f3779ec3d078310127efa64f32bdde9","original_commit_id":"9f123d949f3779ec3d078310127efa64f32bdde9","user":{"login":"hmatsushita","id":15991397,"avatar_url":"https://avatars.githubusercontent.com/u/15991397?v=3","gravatar_id":"","url":"https://api.github.com/users/hmatsushita","html_url":"https://github.com/hmatsushita","followers_url":"https://api.github.com/users/hmatsushita/followers","following_url":"https://api.github.com/users/hmatsushita/following{/other_user}","gists_url":"https://api.github.com/users/hmatsushita/gists{/gist_id}","starred_url":"https://api.github.com/users/hmatsushita/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/hmatsushita/subscriptions","organizations_url":"https://api.github.com/users/hmatsushita/orgs","repos_url":"https://api.github.com/users/hmatsushita/repos","events_url":"https://api.github.com/users/hmatsushita/events{/privacy}","received_events_url":"https://api.github.com/users/hmatsushita/received_events","type":"User","site_admin":false},"body":"遅ればせながら、ご指摘の点に気付きました。\r\ngazebo_ros_bumper プラグインのソースコードを元に実装の改修を行います。","created_at":"2016-12-15T10:51:42Z","updated_at":"2016-12-15T10:51:42Z","html_url":"https://github.com/fkanehiro/choreonoid_ros_pkg/pull/75#discussion_r92585882","pull_request_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75","_links":{"self":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/comments/92585882"},"html":{"href":"https://github.com/fkanehiro/choreonoid_ros_pkg/pull/75#discussion_r92585882"},"pull_request":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75"}}},"pull_request":{"url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75","id":97948246,"html_url":"https://github.com/fkanehiro/choreonoid_ros_pkg/pull/75","diff_url":"https://github.com/fkanehiro/choreonoid_ros_pkg/pull/75.diff","patch_url":"https://github.com/fkanehiro/choreonoid_ros_pkg/pull/75.patch","issue_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues/75","number":75,"state":"open","locked":false,"title":"adds to new feature of the collisions data output","user":{"login":"hmatsushita","id":15991397,"avatar_url":"https://avatars.githubusercontent.com/u/15991397?v=3","gravatar_id":"","url":"https://api.github.com/users/hmatsushita","html_url":"https://github.com/hmatsushita","followers_url":"https://api.github.com/users/hmatsushita/followers","following_url":"https://api.github.com/users/hmatsushita/following{/other_user}","gists_url":"https://api.github.com/users/hmatsushita/gists{/gist_id}","starred_url":"https://api.github.com/users/hmatsushita/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/hmatsushita/subscriptions","organizations_url":"https://api.github.com/users/hmatsushita/orgs","repos_url":"https://api.github.com/users/hmatsushita/repos","events_url":"https://api.github.com/users/hmatsushita/events{/privacy}","received_events_url":"https://api.github.com/users/hmatsushita/received_events","type":"User","site_admin":false},"body":"接触情報の出力を追加しました。\r\n\r\n以下のコマンドで取得することが可能です。\r\n\r\n`$ rostopic echo /choreonoid/[world name]/contacts/physics`\r\n\r\n出力は以下のメッセージタイプを使用しています。\r\n\r\nhttp://docs.ros.org/jade/api/gazebo_msgs/html/msg/ContactState.html\r\n  ","created_at":"2016-12-14T13:05:04Z","updated_at":"2016-12-15T10:51:42Z","closed_at":null,"merged_at":null,"merge_commit_sha":"e6c9147e02e18e1254d656301d0ca69e2233294b","assignee":null,"assignees":[],"milestone":null,"commits_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75/commits","review_comments_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75/comments","review_comment_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/comments{/number}","comments_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues/75/comments","statuses_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/statuses/9f123d949f3779ec3d078310127efa64f32bdde9","head":{"label":"hmatsushita:add_publish_contacts_state","ref":"add_publish_contacts_state","sha":"9f123d949f3779ec3d078310127efa64f32bdde9","user":{"login":"hmatsushita","id":15991397,"avatar_url":"https://avatars.githubusercontent.com/u/15991397?v=3","gravatar_id":"","url":"https://api.github.com/users/hmatsushita","html_url":"https://github.com/hmatsushita","followers_url":"https://api.github.com/users/hmatsushita/followers","following_url":"https://api.github.com/users/hmatsushita/following{/other_user}","gists_url":"https://api.github.com/users/hmatsushita/gists{/gist_id}","starred_url":"https://api.github.com/users/hmatsushita/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/hmatsushita/subscriptions","organizations_url":"https://api.github.com/users/hmatsushita/orgs","repos_url":"https://api.github.com/users/hmatsushita/repos","events_url":"https://api.github.com/users/hmatsushita/events{/privacy}","received_events_url":"https://api.github.com/users/hmatsushita/received_events","type":"User","site_admin":false},"repo":{"id":50071888,"name":"choreonoid_ros_pkg","full_name":"hmatsushita/choreonoid_ros_pkg","owner":{"login":"hmatsushita","id":15991397,"avatar_url":"https://avatars.githubusercontent.com/u/15991397?v=3","gravatar_id":"","url":"https://api.github.com/users/hmatsushita","html_url":"https://github.com/hmatsushita","followers_url":"https://api.github.com/users/hmatsushita/followers","following_url":"https://api.github.com/users/hmatsushita/following{/other_user}","gists_url":"https://api.github.com/users/hmatsushita/gists{/gist_id}","starred_url":"https://api.github.com/users/hmatsushita/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/hmatsushita/subscriptions","organizations_url":"https://api.github.com/users/hmatsushita/orgs","repos_url":"https://api.github.com/users/hmatsushita/repos","events_url":"https://api.github.com/users/hmatsushita/events{/privacy}","received_events_url":"https://api.github.com/users/hmatsushita/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/hmatsushita/choreonoid_ros_pkg","description":null,"fork":true,"url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg","forks_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/forks","keys_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/keys{/key_id}","collaborators_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/teams","hooks_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/hooks","issue_events_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/issues/events{/number}","events_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/events","assignees_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/assignees{/user}","branches_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/branches{/branch}","tags_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/tags","blobs_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/git/refs{/sha}","trees_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/git/trees{/sha}","statuses_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/statuses/{sha}","languages_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/languages","stargazers_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/stargazers","contributors_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/contributors","subscribers_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/subscribers","subscription_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/subscription","commits_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/commits{/sha}","git_commits_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/git/commits{/sha}","comments_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/comments{/number}","issue_comment_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/issues/comments{/number}","contents_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/contents/{+path}","compare_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/compare/{base}...{head}","merges_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/merges","archive_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/downloads","issues_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/issues{/number}","pulls_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/pulls{/number}","milestones_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/milestones{/number}","notifications_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/labels{/name}","releases_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/releases{/id}","deployments_url":"https://api.github.com/repos/hmatsushita/choreonoid_ros_pkg/deployments","created_at":"2016-01-21T01:01:38Z","updated_at":"2016-01-21T01:01:39Z","pushed_at":"2016-12-14T12:59:00Z","git_url":"git://github.com/hmatsushita/choreonoid_ros_pkg.git","ssh_url":"git@github.com:hmatsushita/choreonoid_ros_pkg.git","clone_url":"https://github.com/hmatsushita/choreonoid_ros_pkg.git","svn_url":"https://github.com/hmatsushita/choreonoid_ros_pkg","homepage":null,"size":116,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":false,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"open_issues_count":0,"forks":0,"open_issues":0,"watchers":0,"default_branch":"master"}},"base":{"label":"fkanehiro:master","ref":"master","sha":"4fa9529bfad2362b2a9d32f515ebc021baaa7525","user":{"login":"fkanehiro","id":845257,"avatar_url":"https://avatars.githubusercontent.com/u/845257?v=3","gravatar_id":"","url":"https://api.github.com/users/fkanehiro","html_url":"https://github.com/fkanehiro","followers_url":"https://api.github.com/users/fkanehiro/followers","following_url":"https://api.github.com/users/fkanehiro/following{/other_user}","gists_url":"https://api.github.com/users/fkanehiro/gists{/gist_id}","starred_url":"https://api.github.com/users/fkanehiro/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/fkanehiro/subscriptions","organizations_url":"https://api.github.com/users/fkanehiro/orgs","repos_url":"https://api.github.com/users/fkanehiro/repos","events_url":"https://api.github.com/users/fkanehiro/events{/privacy}","received_events_url":"https://api.github.com/users/fkanehiro/received_events","type":"User","site_admin":false},"repo":{"id":43528489,"name":"choreonoid_ros_pkg","full_name":"fkanehiro/choreonoid_ros_pkg","owner":{"login":"fkanehiro","id":845257,"avatar_url":"https://avatars.githubusercontent.com/u/845257?v=3","gravatar_id":"","url":"https://api.github.com/users/fkanehiro","html_url":"https://github.com/fkanehiro","followers_url":"https://api.github.com/users/fkanehiro/followers","following_url":"https://api.github.com/users/fkanehiro/following{/other_user}","gists_url":"https://api.github.com/users/fkanehiro/gists{/gist_id}","starred_url":"https://api.github.com/users/fkanehiro/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/fkanehiro/subscriptions","organizations_url":"https://api.github.com/users/fkanehiro/orgs","repos_url":"https://api.github.com/users/fkanehiro/repos","events_url":"https://api.github.com/users/fkanehiro/events{/privacy}","received_events_url":"https://api.github.com/users/fkanehiro/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/fkanehiro/choreonoid_ros_pkg","description":null,"fork":false,"url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg","forks_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/forks","keys_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/keys{/key_id}","collaborators_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/teams","hooks_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/hooks","issue_events_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues/events{/number}","events_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/events","assignees_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/assignees{/user}","branches_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/branches{/branch}","tags_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/tags","blobs_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/git/refs{/sha}","trees_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/git/trees{/sha}","statuses_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/statuses/{sha}","languages_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/languages","stargazers_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/stargazers","contributors_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/contributors","subscribers_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/subscribers","subscription_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/subscription","commits_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/commits{/sha}","git_commits_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/git/commits{/sha}","comments_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/comments{/number}","issue_comment_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues/comments{/number}","contents_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/contents/{+path}","compare_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/compare/{base}...{head}","merges_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/merges","archive_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/downloads","issues_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues{/number}","pulls_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls{/number}","milestones_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/milestones{/number}","notifications_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/labels{/name}","releases_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/releases{/id}","deployments_url":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/deployments","created_at":"2015-10-02T00:27:27Z","updated_at":"2016-01-21T05:31:30Z","pushed_at":"2016-12-14T13:05:05Z","git_url":"git://github.com/fkanehiro/choreonoid_ros_pkg.git","ssh_url":"git@github.com:fkanehiro/choreonoid_ros_pkg.git","clone_url":"https://github.com/fkanehiro/choreonoid_ros_pkg.git","svn_url":"https://github.com/fkanehiro/choreonoid_ros_pkg","homepage":null,"size":131,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":3,"mirror_url":null,"open_issues_count":12,"forks":3,"open_issues":12,"watchers":0,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75"},"html":{"href":"https://github.com/fkanehiro/choreonoid_ros_pkg/pull/75"},"issue":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues/75"},"comments":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/issues/75/comments"},"review_comments":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75/comments"},"review_comment":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/pulls/75/commits"},"statuses":{"href":"https://api.github.com/repos/fkanehiro/choreonoid_ros_pkg/statuses/9f123d949f3779ec3d078310127efa64f32bdde9"}}}}
{"actor":{"display_login":"hmatsushita"}}