{"action":"created","comment":{"url":"https://api.github.com/repos/6RiverSystems/navigation/pulls/comments/94030836","pull_request_review_id":14560790,"id":94030836,"diff_hunk":"@@ -0,0 +1,131 @@\n+/*********************************************************************\n+ *\n+ * Software License Agreement (BSD License)\n+ *\n+ *  Copyright (c) 2016, 6 River Systems\n+ *  All rights reserved.\n+ *\n+ *  Redistribution and use in source and binary forms, with or without\n+ *  modification, are permitted provided that the following conditions\n+ *  are met:\n+ *\n+ *   * Redistributions of source code must retain the above copyright\n+ *     notice, this list of conditions and the following disclaimer.\n+ *   * Redistributions in binary form must reproduce the above\n+ *     copyright notice, this list of conditions and the following\n+ *     disclaimer in the documentation and/or other materials provided\n+ *     with the distribution.\n+ *   * Neither the name of Willow Garage, Inc. nor the names of its\n+ *     contributors may be used to endorse or promote products derived\n+ *     from this software without specific prior written permission.\n+ *\n+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n+ *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n+ *  POSSIBILITY OF SUCH DAMAGE.\n+ *\n+ * Author: Daniel Grieneisen\n+ *********************************************************************/\n+\n+#include <base_local_planner/heading_cost_function.h>\n+#include <tf/transform_datatypes.h>\n+\n+namespace base_local_planner {\n+\n+HeadingCostFunction::HeadingCostFunction(double rejection_half_angle) :\n+    rejection_half_angle_(rejection_half_angle),\n+    path_start_heading_(0), EPSILON(0.001),\n+    goal_distance_squared_(0.0),\n+    min_goal_distance_(0.5),\n+    pose_capture_min_radius_(0.5), pose_capture_max_radius_(1.5),\n+    heading_violation_(false) {}\n+\n+void HeadingCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {\n+  target_poses_ = target_poses;\n+}\n+\n+bool HeadingCostFunction::prepare() {\n+\n+  heading_violation_ = false;\n+\n+  if (goal_distance_squared_ < (min_goal_distance_ * min_goal_distance_))\n+  {\n+    heading_violation_ = false;\n+    return true;\n+  }\n+\n+  if (target_poses_.size() == 0)\n+  {\n+    heading_violation_ = false;\n+    return true;\n+  }\n+\n+  // Calculate the 'best' heading to compare to.\n+  double min_caputure_radius_squared = pose_capture_min_radius_ * pose_capture_min_radius_;\n+  double max_caputure_radius_squared = pose_capture_max_radius_ * pose_capture_max_radius_;\n+\n+  // Go forwards through the poses and find the first one that exits the min radius.\n+  geometry_msgs::PoseStamped target_pose = target_poses_[0];\n+  for (size_t k = 0; k < target_poses_.size(); ++k)\n+  {\n+    double dist = poseDistanceSquared(target_poses_[k], current_pose_);\n+    if (dist < min_caputure_radius_squared)\n+    {\n+      target_pose = target_poses_[k];\n+    }\n+    else if (dist > max_caputure_radius_squared)\n+    {\n+        break;\n+    }\n+  }\n+\n+  path_start_heading_ = tf::getYaw(target_pose.pose.orientation);\n+\n+  // Check the heading difference\n+  double current_heading = tf::getYaw(current_pose_.pose.orientation);\n+  double heading_diff = current_heading - path_start_heading_;\n+  // Normalize the heading difference\n+  while (heading_diff > M_PI)\n+  {\n+    heading_diff -= 2 * M_PI;\n+  }\n+  while (heading_diff < -M_PI)\n+  {\n+    heading_diff += 2 * M_PI;\n+  }\n+\n+  ROS_DEBUG(\"Path heading: %f, current heading: %f\", path_start_heading_, current_heading);\n+  if (std::fabs(heading_diff) > rejection_half_angle_)\n+  {\n+    heading_violation_ = true;\n+  }\n+\n+  return true;\n+}\n+\n+double HeadingCostFunction::scoreTrajectory(Trajectory &traj) {\n+\n+  if (heading_violation_)\n+  {\n+    // Check to see if this is a turn in place motion.\n+    if (std::fabs(traj.thetav_) < EPSILON)\n+    {\n+      return -1.0;","path":"base_local_planner/src/heading_cost_function.cpp","position":121,"original_position":121,"commit_id":"de0e12aee64fde607ebfb2ac657679f5d2cdeaa3","original_commit_id":"de0e12aee64fde607ebfb2ac657679f5d2cdeaa3","user":{"login":"dantwinkler","id":2089359,"avatar_url":"https://avatars.githubusercontent.com/u/2089359?v=3","gravatar_id":"","url":"https://api.github.com/users/dantwinkler","html_url":"https://github.com/dantwinkler","followers_url":"https://api.github.com/users/dantwinkler/followers","following_url":"https://api.github.com/users/dantwinkler/following{/other_user}","gists_url":"https://api.github.com/users/dantwinkler/gists{/gist_id}","starred_url":"https://api.github.com/users/dantwinkler/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/dantwinkler/subscriptions","organizations_url":"https://api.github.com/users/dantwinkler/orgs","repos_url":"https://api.github.com/users/dantwinkler/repos","events_url":"https://api.github.com/users/dantwinkler/events{/privacy}","received_events_url":"https://api.github.com/users/dantwinkler/received_events","type":"User","site_admin":false},"body":"Are these defined somewhere?  If not maybe comment what they are supposed to achieve (invalid trajectory, etc.).","created_at":"2016-12-28T13:10:03Z","updated_at":"2016-12-28T13:10:03Z","html_url":"https://github.com/6RiverSystems/navigation/pull/12#discussion_r94030836","pull_request_url":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12","_links":{"self":{"href":"https://api.github.com/repos/6RiverSystems/navigation/pulls/comments/94030836"},"html":{"href":"https://github.com/6RiverSystems/navigation/pull/12#discussion_r94030836"},"pull_request":{"href":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12"}}},"pull_request":{"url":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12","id":99010244,"html_url":"https://github.com/6RiverSystems/navigation/pull/12","diff_url":"https://github.com/6RiverSystems/navigation/pull/12.diff","patch_url":"https://github.com/6RiverSystems/navigation/pull/12.patch","issue_url":"https://api.github.com/repos/6RiverSystems/navigation/issues/12","number":12,"state":"open","locked":false,"title":"Fixes to DWALocalPlanner for better obstacle avoidance.","user":{"login":"grieneis","id":10405663,"avatar_url":"https://avatars.githubusercontent.com/u/10405663?v=3","gravatar_id":"","url":"https://api.github.com/users/grieneis","html_url":"https://github.com/grieneis","followers_url":"https://api.github.com/users/grieneis/followers","following_url":"https://api.github.com/users/grieneis/following{/other_user}","gists_url":"https://api.github.com/users/grieneis/gists{/gist_id}","starred_url":"https://api.github.com/users/grieneis/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/grieneis/subscriptions","organizations_url":"https://api.github.com/users/grieneis/orgs","repos_url":"https://api.github.com/users/grieneis/repos","events_url":"https://api.github.com/users/grieneis/events{/privacy}","received_events_url":"https://api.github.com/users/grieneis/received_events","type":"User","site_admin":false},"body":"DO NOT MERGE YET.","created_at":"2016-12-21T21:45:32Z","updated_at":"2016-12-28T13:10:03Z","closed_at":null,"merged_at":null,"merge_commit_sha":"83373d02ede4c44e4d4ed572203fbef4ef2b4005","assignee":null,"assignees":[],"milestone":null,"commits_url":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12/commits","review_comments_url":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12/comments","review_comment_url":"https://api.github.com/repos/6RiverSystems/navigation/pulls/comments{/number}","comments_url":"https://api.github.com/repos/6RiverSystems/navigation/issues/12/comments","statuses_url":"https://api.github.com/repos/6RiverSystems/navigation/statuses/de0e12aee64fde607ebfb2ac657679f5d2cdeaa3","head":{"label":"6RiverSystems:feature/obstacle-avoidance","ref":"feature/obstacle-avoidance","sha":"de0e12aee64fde607ebfb2ac657679f5d2cdeaa3","user":{"login":"6RiverSystems","id":15053376,"avatar_url":"https://avatars.githubusercontent.com/u/15053376?v=3","gravatar_id":"","url":"https://api.github.com/users/6RiverSystems","html_url":"https://github.com/6RiverSystems","followers_url":"https://api.github.com/users/6RiverSystems/followers","following_url":"https://api.github.com/users/6RiverSystems/following{/other_user}","gists_url":"https://api.github.com/users/6RiverSystems/gists{/gist_id}","starred_url":"https://api.github.com/users/6RiverSystems/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/6RiverSystems/subscriptions","organizations_url":"https://api.github.com/users/6RiverSystems/orgs","repos_url":"https://api.github.com/users/6RiverSystems/repos","events_url":"https://api.github.com/users/6RiverSystems/events{/privacy}","received_events_url":"https://api.github.com/users/6RiverSystems/received_events","type":"Organization","site_admin":false},"repo":{"id":71017104,"name":"navigation","full_name":"6RiverSystems/navigation","owner":{"login":"6RiverSystems","id":15053376,"avatar_url":"https://avatars.githubusercontent.com/u/15053376?v=3","gravatar_id":"","url":"https://api.github.com/users/6RiverSystems","html_url":"https://github.com/6RiverSystems","followers_url":"https://api.github.com/users/6RiverSystems/followers","following_url":"https://api.github.com/users/6RiverSystems/following{/other_user}","gists_url":"https://api.github.com/users/6RiverSystems/gists{/gist_id}","starred_url":"https://api.github.com/users/6RiverSystems/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/6RiverSystems/subscriptions","organizations_url":"https://api.github.com/users/6RiverSystems/orgs","repos_url":"https://api.github.com/users/6RiverSystems/repos","events_url":"https://api.github.com/users/6RiverSystems/events{/privacy}","received_events_url":"https://api.github.com/users/6RiverSystems/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/6RiverSystems/navigation","description":"ROS Navigation stack.  Code for finding where the robot is and how it can get somewhere else.","fork":true,"url":"https://api.github.com/repos/6RiverSystems/navigation","forks_url":"https://api.github.com/repos/6RiverSystems/navigation/forks","keys_url":"https://api.github.com/repos/6RiverSystems/navigation/keys{/key_id}","collaborators_url":"https://api.github.com/repos/6RiverSystems/navigation/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/6RiverSystems/navigation/teams","hooks_url":"https://api.github.com/repos/6RiverSystems/navigation/hooks","issue_events_url":"https://api.github.com/repos/6RiverSystems/navigation/issues/events{/number}","events_url":"https://api.github.com/repos/6RiverSystems/navigation/events","assignees_url":"https://api.github.com/repos/6RiverSystems/navigation/assignees{/user}","branches_url":"https://api.github.com/repos/6RiverSystems/navigation/branches{/branch}","tags_url":"https://api.github.com/repos/6RiverSystems/navigation/tags","blobs_url":"https://api.github.com/repos/6RiverSystems/navigation/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/6RiverSystems/navigation/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/6RiverSystems/navigation/git/refs{/sha}","trees_url":"https://api.github.com/repos/6RiverSystems/navigation/git/trees{/sha}","statuses_url":"https://api.github.com/repos/6RiverSystems/navigation/statuses/{sha}","languages_url":"https://api.github.com/repos/6RiverSystems/navigation/languages","stargazers_url":"https://api.github.com/repos/6RiverSystems/navigation/stargazers","contributors_url":"https://api.github.com/repos/6RiverSystems/navigation/contributors","subscribers_url":"https://api.github.com/repos/6RiverSystems/navigation/subscribers","subscription_url":"https://api.github.com/repos/6RiverSystems/navigation/subscription","commits_url":"https://api.github.com/repos/6RiverSystems/navigation/commits{/sha}","git_commits_url":"https://api.github.com/repos/6RiverSystems/navigation/git/commits{/sha}","comments_url":"https://api.github.com/repos/6RiverSystems/navigation/comments{/number}","issue_comment_url":"https://api.github.com/repos/6RiverSystems/navigation/issues/comments{/number}","contents_url":"https://api.github.com/repos/6RiverSystems/navigation/contents/{+path}","compare_url":"https://api.github.com/repos/6RiverSystems/navigation/compare/{base}...{head}","merges_url":"https://api.github.com/repos/6RiverSystems/navigation/merges","archive_url":"https://api.github.com/repos/6RiverSystems/navigation/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/6RiverSystems/navigation/downloads","issues_url":"https://api.github.com/repos/6RiverSystems/navigation/issues{/number}","pulls_url":"https://api.github.com/repos/6RiverSystems/navigation/pulls{/number}","milestones_url":"https://api.github.com/repos/6RiverSystems/navigation/milestones{/number}","notifications_url":"https://api.github.com/repos/6RiverSystems/navigation/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/6RiverSystems/navigation/labels{/name}","releases_url":"https://api.github.com/repos/6RiverSystems/navigation/releases{/id}","deployments_url":"https://api.github.com/repos/6RiverSystems/navigation/deployments","created_at":"2016-10-15T22:31:21Z","updated_at":"2016-10-15T22:31:23Z","pushed_at":"2016-12-23T18:35:32Z","git_url":"git://github.com/6RiverSystems/navigation.git","ssh_url":"git@github.com:6RiverSystems/navigation.git","clone_url":"https://github.com/6RiverSystems/navigation.git","svn_url":"https://github.com/6RiverSystems/navigation","homepage":null,"size":5295,"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":1,"forks":0,"open_issues":1,"watchers":0,"default_branch":"jade-devel"}},"base":{"label":"6RiverSystems:indigo-devel","ref":"indigo-devel","sha":"896c62b76921bbd83f63930c21c74d2cfd290ae1","user":{"login":"6RiverSystems","id":15053376,"avatar_url":"https://avatars.githubusercontent.com/u/15053376?v=3","gravatar_id":"","url":"https://api.github.com/users/6RiverSystems","html_url":"https://github.com/6RiverSystems","followers_url":"https://api.github.com/users/6RiverSystems/followers","following_url":"https://api.github.com/users/6RiverSystems/following{/other_user}","gists_url":"https://api.github.com/users/6RiverSystems/gists{/gist_id}","starred_url":"https://api.github.com/users/6RiverSystems/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/6RiverSystems/subscriptions","organizations_url":"https://api.github.com/users/6RiverSystems/orgs","repos_url":"https://api.github.com/users/6RiverSystems/repos","events_url":"https://api.github.com/users/6RiverSystems/events{/privacy}","received_events_url":"https://api.github.com/users/6RiverSystems/received_events","type":"Organization","site_admin":false},"repo":{"id":71017104,"name":"navigation","full_name":"6RiverSystems/navigation","owner":{"login":"6RiverSystems","id":15053376,"avatar_url":"https://avatars.githubusercontent.com/u/15053376?v=3","gravatar_id":"","url":"https://api.github.com/users/6RiverSystems","html_url":"https://github.com/6RiverSystems","followers_url":"https://api.github.com/users/6RiverSystems/followers","following_url":"https://api.github.com/users/6RiverSystems/following{/other_user}","gists_url":"https://api.github.com/users/6RiverSystems/gists{/gist_id}","starred_url":"https://api.github.com/users/6RiverSystems/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/6RiverSystems/subscriptions","organizations_url":"https://api.github.com/users/6RiverSystems/orgs","repos_url":"https://api.github.com/users/6RiverSystems/repos","events_url":"https://api.github.com/users/6RiverSystems/events{/privacy}","received_events_url":"https://api.github.com/users/6RiverSystems/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/6RiverSystems/navigation","description":"ROS Navigation stack.  Code for finding where the robot is and how it can get somewhere else.","fork":true,"url":"https://api.github.com/repos/6RiverSystems/navigation","forks_url":"https://api.github.com/repos/6RiverSystems/navigation/forks","keys_url":"https://api.github.com/repos/6RiverSystems/navigation/keys{/key_id}","collaborators_url":"https://api.github.com/repos/6RiverSystems/navigation/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/6RiverSystems/navigation/teams","hooks_url":"https://api.github.com/repos/6RiverSystems/navigation/hooks","issue_events_url":"https://api.github.com/repos/6RiverSystems/navigation/issues/events{/number}","events_url":"https://api.github.com/repos/6RiverSystems/navigation/events","assignees_url":"https://api.github.com/repos/6RiverSystems/navigation/assignees{/user}","branches_url":"https://api.github.com/repos/6RiverSystems/navigation/branches{/branch}","tags_url":"https://api.github.com/repos/6RiverSystems/navigation/tags","blobs_url":"https://api.github.com/repos/6RiverSystems/navigation/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/6RiverSystems/navigation/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/6RiverSystems/navigation/git/refs{/sha}","trees_url":"https://api.github.com/repos/6RiverSystems/navigation/git/trees{/sha}","statuses_url":"https://api.github.com/repos/6RiverSystems/navigation/statuses/{sha}","languages_url":"https://api.github.com/repos/6RiverSystems/navigation/languages","stargazers_url":"https://api.github.com/repos/6RiverSystems/navigation/stargazers","contributors_url":"https://api.github.com/repos/6RiverSystems/navigation/contributors","subscribers_url":"https://api.github.com/repos/6RiverSystems/navigation/subscribers","subscription_url":"https://api.github.com/repos/6RiverSystems/navigation/subscription","commits_url":"https://api.github.com/repos/6RiverSystems/navigation/commits{/sha}","git_commits_url":"https://api.github.com/repos/6RiverSystems/navigation/git/commits{/sha}","comments_url":"https://api.github.com/repos/6RiverSystems/navigation/comments{/number}","issue_comment_url":"https://api.github.com/repos/6RiverSystems/navigation/issues/comments{/number}","contents_url":"https://api.github.com/repos/6RiverSystems/navigation/contents/{+path}","compare_url":"https://api.github.com/repos/6RiverSystems/navigation/compare/{base}...{head}","merges_url":"https://api.github.com/repos/6RiverSystems/navigation/merges","archive_url":"https://api.github.com/repos/6RiverSystems/navigation/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/6RiverSystems/navigation/downloads","issues_url":"https://api.github.com/repos/6RiverSystems/navigation/issues{/number}","pulls_url":"https://api.github.com/repos/6RiverSystems/navigation/pulls{/number}","milestones_url":"https://api.github.com/repos/6RiverSystems/navigation/milestones{/number}","notifications_url":"https://api.github.com/repos/6RiverSystems/navigation/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/6RiverSystems/navigation/labels{/name}","releases_url":"https://api.github.com/repos/6RiverSystems/navigation/releases{/id}","deployments_url":"https://api.github.com/repos/6RiverSystems/navigation/deployments","created_at":"2016-10-15T22:31:21Z","updated_at":"2016-10-15T22:31:23Z","pushed_at":"2016-12-23T18:35:32Z","git_url":"git://github.com/6RiverSystems/navigation.git","ssh_url":"git@github.com:6RiverSystems/navigation.git","clone_url":"https://github.com/6RiverSystems/navigation.git","svn_url":"https://github.com/6RiverSystems/navigation","homepage":null,"size":5295,"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":1,"forks":0,"open_issues":1,"watchers":0,"default_branch":"jade-devel"}},"_links":{"self":{"href":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12"},"html":{"href":"https://github.com/6RiverSystems/navigation/pull/12"},"issue":{"href":"https://api.github.com/repos/6RiverSystems/navigation/issues/12"},"comments":{"href":"https://api.github.com/repos/6RiverSystems/navigation/issues/12/comments"},"review_comments":{"href":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12/comments"},"review_comment":{"href":"https://api.github.com/repos/6RiverSystems/navigation/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/6RiverSystems/navigation/pulls/12/commits"},"statuses":{"href":"https://api.github.com/repos/6RiverSystems/navigation/statuses/de0e12aee64fde607ebfb2ac657679f5d2cdeaa3"}}}}
{"actor":{"display_login":"dantwinkler"}}