00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <planner_cspace_msgs/PlannerStatus.h>
00032 #include <trajectory_msgs/JointTrajectory.h>
00033 #include <sensor_msgs/JointState.h>
00034 #include <tf2_ros/transform_listener.h>
00035
00036 #include <utility>
00037 #include <algorithm>
00038 #include <string>
00039 #include <list>
00040 #include <vector>
00041
00042 #include <planner_cspace/grid_astar.h>
00043
00044 #include <neonavigation_common/compatibility.h>
00045
00046 #include <omp.h>
00047
00048 class planner2dofSerialJointsNode
00049 {
00050 public:
00051 using Astar = GridAstar<2, 0>;
00052
00053 private:
00054 ros::NodeHandle nh_;
00055 ros::NodeHandle pnh_;
00056
00057 ros::Publisher pub_status_;
00058 ros::Publisher pub_trajectory_;
00059 ros::Subscriber sub_trajectory_;
00060 ros::Subscriber sub_joint_;
00061
00062 tf2_ros::Buffer tfbuf_;
00063 tf2_ros::TransformListener tfl_;
00064
00065 Astar as_;
00066 Astar::Gridmap<char, 0x40> cm_;
00067
00068 Astar::Vecf euclid_cost_coef_;
00069
00070 float euclidCost(const Astar::Vec& v, const Astar::Vecf coef)
00071 {
00072 auto vc = v;
00073 float cost = 0;
00074 for (int i = 0; i < as_.getDim(); i++)
00075 {
00076 cost += fabs(coef[i] * vc[i]);
00077 }
00078 return cost;
00079 }
00080 float euclidCost(const Astar::Vec& v)
00081 {
00082 return euclidCost(v, euclid_cost_coef_);
00083 }
00084
00085 float freq_;
00086 float freq_min_;
00087 bool has_goal_;
00088 bool has_start_;
00089 std::vector<Astar::Vec> search_list_;
00090 int resolution_;
00091 float weight_cost_;
00092 float expand_;
00093 float avg_vel_;
00094 enum PointVelMode
00095 {
00096 VEL_PREV,
00097 VEL_NEXT,
00098 VEL_AVG
00099 };
00100 PointVelMode point_vel_;
00101
00102 std::string group_;
00103
00104 bool debug_aa_;
00105
00106 class LinkBody
00107 {
00108 protected:
00109 class Vec3dof
00110 {
00111 public:
00112 float x_;
00113 float y_;
00114 float th_;
00115
00116 float dist(const Vec3dof& b)
00117 {
00118 return hypotf(b.x_ - x_, b.y_ - y_);
00119 }
00120 };
00121
00122 public:
00123 float radius_[2];
00124 float vmax_;
00125 float length_;
00126 std::string name_;
00127 Vec3dof origin_;
00128 Vec3dof gain_;
00129 float current_th_;
00130
00131 LinkBody()
00132 {
00133 gain_.x_ = 1.0;
00134 gain_.y_ = 1.0;
00135 gain_.th_ = 1.0;
00136 }
00137 Vec3dof end(const float th) const
00138 {
00139 Vec3dof e = origin_;
00140 e.x_ += cosf(e.th_ + th * gain_.th_) * length_;
00141 e.y_ += sinf(e.th_ + th * gain_.th_) * length_;
00142 e.th_ += th;
00143 return e;
00144 }
00145 bool isCollide(const LinkBody b, const float th0, const float th1)
00146 {
00147 auto end0 = end(th0);
00148 auto end1 = b.end(th1);
00149 auto& end0r = radius_[1];
00150 auto& end1r = b.radius_[1];
00151 auto& origin0 = origin_;
00152 auto& origin1 = b.origin_;
00153 auto& origin0r = radius_[0];
00154 auto& origin1r = b.radius_[0];
00155
00156 if (end0.dist(end1) < end0r + end1r)
00157 return true;
00158 if (end0.dist(origin1) < end0r + origin1r)
00159 return true;
00160 if (end1.dist(origin0) < end1r + origin0r)
00161 return true;
00162
00163
00164
00165 return false;
00166 }
00167 };
00168 LinkBody links_[2];
00169
00170 planner_cspace_msgs::PlannerStatus status_;
00171 sensor_msgs::JointState joint_;
00172 ros::Time replan_prev_;
00173 ros::Duration replan_interval_;
00174
00175 void cbJoint(const sensor_msgs::JointState::ConstPtr& msg)
00176 {
00177 int id[2] = { -1, -1 };
00178 for (size_t i = 0; i < msg->name.size(); i++)
00179 {
00180 if (msg->name[i].compare(links_[0].name_) == 0)
00181 id[0] = i;
00182 else if (msg->name[i].compare(links_[1].name_) == 0)
00183 id[1] = i;
00184 }
00185 if (id[0] == -1 || id[1] == -1)
00186 {
00187 ROS_ERROR("joint_state does not contain link group %s.", group_.c_str());
00188 return;
00189 }
00190 links_[0].current_th_ = msg->position[id[0]];
00191 links_[1].current_th_ = msg->position[id[1]];
00192
00193 if (replan_prev_ + replan_interval_ < ros::Time::now() &&
00194 replan_interval_ > ros::Duration(0) &&
00195 replan_prev_ != ros::Time(0))
00196 {
00197 replan();
00198 }
00199 }
00200 std::pair<ros::Duration, std::pair<float, float>> cmd_prev;
00201 trajectory_msgs::JointTrajectory traj_prev;
00202 int id[2];
00203 void cbTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr& msg)
00204 {
00205 id[0] = -1;
00206 id[1] = -1;
00207 for (size_t i = 0; i < msg->joint_names.size(); i++)
00208 {
00209 if (msg->joint_names[i].compare(links_[0].name_) == 0)
00210 id[0] = i;
00211 else if (msg->joint_names[i].compare(links_[1].name_) == 0)
00212 id[1] = i;
00213 }
00214 if (id[0] == -1 || id[1] == -1)
00215 {
00216 ROS_ERROR("joint_trajectory does not contains link group %s.", group_.c_str());
00217 return;
00218 }
00219 if (msg->points.size() != 1)
00220 {
00221 ROS_ERROR("single trajectory point required.");
00222 }
00223 decltype(cmd_prev) cmd;
00224 cmd.first = msg->points[0].time_from_start;
00225 cmd.second.first = msg->points[0].positions[id[0]];
00226 cmd.second.second = msg->points[0].positions[id[1]];
00227 if (cmd_prev == cmd)
00228 return;
00229 cmd_prev = cmd;
00230 traj_prev = *msg;
00231 avg_vel_ = -1.0;
00232
00233 replan();
00234 }
00235 void replan()
00236 {
00237 replan_prev_ = ros::Time::now();
00238 if (id[0] == -1 || id[1] == -1)
00239 return;
00240
00241 Astar::Vecf start(
00242 links_[0].current_th_,
00243 links_[1].current_th_);
00244 Astar::Vecf end(
00245 static_cast<float>(traj_prev.points[0].positions[id[0]]),
00246 static_cast<float>(traj_prev.points[0].positions[id[1]]));
00247
00248 ROS_INFO("link %s: %0.3f, %0.3f", group_.c_str(),
00249 traj_prev.points[0].positions[id[0]],
00250 traj_prev.points[0].positions[id[1]]);
00251
00252 ROS_INFO("Start searching");
00253 std::list<Astar::Vecf> path;
00254 if (makePlan(start, end, path))
00255 {
00256 ROS_INFO("Trajectory found");
00257
00258 if (avg_vel_ < 0)
00259 {
00260 float pos_sum = 0;
00261 for (auto it = path.begin(); it != path.end(); it++)
00262 {
00263 auto it_next = it;
00264 it_next++;
00265 if (it_next != path.end())
00266 {
00267 float diff[2], diff_max;
00268 diff[0] = fabs((*it_next)[0] - (*it)[0]);
00269 diff[1] = fabs((*it_next)[1] - (*it)[1]);
00270 diff_max = std::max(diff[0], diff[1]);
00271 pos_sum += diff_max;
00272 }
00273 }
00274 if (traj_prev.points[0].time_from_start <= ros::Duration(0))
00275 {
00276 avg_vel_ = std::min(links_[0].vmax_, links_[1].vmax_);
00277 }
00278 else
00279 {
00280 avg_vel_ = pos_sum / traj_prev.points[0].time_from_start.toSec();
00281 if (avg_vel_ > links_[0].vmax_)
00282 avg_vel_ = links_[0].vmax_;
00283 if (avg_vel_ > links_[1].vmax_)
00284 avg_vel_ = links_[1].vmax_;
00285 }
00286 }
00287
00288 trajectory_msgs::JointTrajectory out;
00289 out.header = traj_prev.header;
00290 out.header.stamp = ros::Time(0);
00291 out.joint_names.resize(2);
00292 out.joint_names[0] = links_[0].name_;
00293 out.joint_names[1] = links_[1].name_;
00294 float pos_sum = 0.0;
00295 for (auto it = path.begin(); it != path.end(); it++)
00296 {
00297 if (it == path.begin())
00298 continue;
00299
00300 trajectory_msgs::JointTrajectoryPoint p;
00301 p.positions.resize(2);
00302 p.velocities.resize(2);
00303
00304 auto it_prev = it;
00305 it_prev--;
00306 auto it_next = it;
00307 it_next++;
00308
00309 float diff[2], diff_max;
00310 diff[0] = fabs((*it)[0] - (*it_prev)[0]);
00311 diff[1] = fabs((*it)[1] - (*it_prev)[1]);
00312 diff_max = std::max(diff[0], diff[1]);
00313 pos_sum += diff_max;
00314
00315 if (it_next == path.end())
00316 {
00317 p.velocities[0] = 0.0;
00318 p.velocities[1] = 0.0;
00319 }
00320 else
00321 {
00322 float dir[2], dir_max;
00323 switch (point_vel_)
00324 {
00325 default:
00326 case VEL_PREV:
00327 dir[0] = ((*it)[0] - (*it_prev)[0]);
00328 dir[1] = ((*it)[1] - (*it_prev)[1]);
00329 break;
00330 case VEL_NEXT:
00331 dir[0] = ((*it_next)[0] - (*it)[0]);
00332 dir[1] = ((*it_next)[1] - (*it)[1]);
00333 break;
00334 case VEL_AVG:
00335 dir[0] = ((*it_next)[0] - (*it_prev)[0]);
00336 dir[1] = ((*it_next)[1] - (*it_prev)[1]);
00337 break;
00338 }
00339 dir_max = std::max(fabs(dir[0]), fabs(dir[1]));
00340 float t = dir_max / avg_vel_;
00341
00342 p.velocities[0] = dir[0] / t;
00343 p.velocities[1] = dir[1] / t;
00344 }
00345 p.time_from_start = ros::Duration(pos_sum / avg_vel_);
00346 p.positions[0] = (*it)[0];
00347 p.positions[1] = (*it)[1];
00348 out.points.push_back(p);
00349 }
00350 pub_trajectory_.publish(out);
00351 }
00352 else
00353 {
00354 trajectory_msgs::JointTrajectory out;
00355 out.header = traj_prev.header;
00356 out.header.stamp = ros::Time(0);
00357 out.joint_names.resize(2);
00358 out.joint_names[0] = links_[0].name_;
00359 out.joint_names[1] = links_[1].name_;
00360 trajectory_msgs::JointTrajectoryPoint p;
00361 p.positions.resize(2);
00362 p.positions[0] = links_[0].current_th_;
00363 p.positions[1] = links_[1].current_th_;
00364 p.velocities.resize(2);
00365 out.points.push_back(p);
00366 pub_trajectory_.publish(out);
00367
00368 ROS_WARN("Trajectory not found");
00369 }
00370
00371 pub_status_.publish(status_);
00372 }
00373
00374 public:
00375 explicit planner2dofSerialJointsNode(const std::string group_name)
00376 : nh_()
00377 , pnh_("~")
00378 , tfl_(tfbuf_)
00379 {
00380 neonavigation_common::compat::checkCompatMode();
00381 group_ = group_name;
00382 ros::NodeHandle nh_group("~/" + group_);
00383
00384 pub_trajectory_ = neonavigation_common::compat::advertise<trajectory_msgs::JointTrajectory>(
00385 nh_, "joint_trajectory",
00386 pnh_, "trajectory_out", 1, true);
00387 sub_trajectory_ = neonavigation_common::compat::subscribe(
00388 nh_, "trajectory_in",
00389 pnh_, "trajectory_in", 1, &planner2dofSerialJointsNode::cbTrajectory, this);
00390 sub_joint_ = neonavigation_common::compat::subscribe(
00391 nh_, "joint_states",
00392 pnh_, "joint", 1, &planner2dofSerialJointsNode::cbJoint, this);
00393
00394 pub_status_ = nh_group.advertise<planner_cspace_msgs::PlannerStatus>("status", 1, true);
00395
00396 nh_group.param("resolution", resolution_, 128);
00397 pnh_.param("debug_aa", debug_aa_, false);
00398
00399 double interval;
00400 pnh_.param("replan_interval", interval, 0.2);
00401 replan_interval_ = ros::Duration(interval);
00402 replan_prev_ = ros::Time(0);
00403
00404 int queue_size_limit;
00405 nh_group.param("queue_size_limit", queue_size_limit, 0);
00406 as_.setQueueSizeLimit(queue_size_limit);
00407
00408 status_.status = planner_cspace_msgs::PlannerStatus::DONE;
00409
00410 has_goal_ = false;
00411 has_start_ = false;
00412
00413 cm_.reset(Astar::Vec(resolution_ * 2, resolution_ * 2));
00414 as_.reset(Astar::Vec(resolution_ * 2, resolution_ * 2));
00415 cm_.clear(0);
00416
00417 nh_group.param("link0_name", links_[0].name_, std::string("link0"));
00418 nh_group.param("link0_joint_radius", links_[0].radius_[0], 0.07f);
00419 nh_group.param("link0_end_radius", links_[0].radius_[1], 0.07f);
00420 nh_group.param("link0_length", links_[0].length_, 0.135f);
00421 nh_group.param("link0_x", links_[0].origin_.x_, 0.22f);
00422 nh_group.param("link0_y", links_[0].origin_.y_, 0.0f);
00423 nh_group.param("link0_th", links_[0].origin_.th_, 0.0f);
00424 nh_group.param("link0_gain_th", links_[0].gain_.th_, -1.0f);
00425 nh_group.param("link0_vmax", links_[0].vmax_, 0.5f);
00426 nh_group.param("link1_name", links_[1].name_, std::string("link1"));
00427 nh_group.param("link1_joint_radius", links_[1].radius_[0], 0.07f);
00428 nh_group.param("link1_end_radius", links_[1].radius_[1], 0.07f);
00429 nh_group.param("link1_length", links_[1].length_, 0.27f);
00430 nh_group.param("link1_x", links_[1].origin_.x_, -0.22f);
00431 nh_group.param("link1_y", links_[1].origin_.y_, 0.0f);
00432 nh_group.param("link1_th", links_[1].origin_.th_, 0.0f);
00433 nh_group.param("link1_gain_th", links_[1].gain_.th_, 1.0f);
00434 nh_group.param("link1_vmax", links_[1].vmax_, 0.5f);
00435
00436 links_[0].current_th_ = 0.0;
00437 links_[1].current_th_ = 0.0;
00438
00439 ROS_INFO("link group: %s", group_.c_str());
00440 ROS_INFO(" - link0: %s", links_[0].name_.c_str());
00441 ROS_INFO(" - link1: %s", links_[1].name_.c_str());
00442
00443 nh_group.param("link0_coef", euclid_cost_coef_[0], 1.0f);
00444 nh_group.param("link1_coef", euclid_cost_coef_[1], 1.5f);
00445
00446 nh_group.param("weight_cost", weight_cost_, 4.0f);
00447 nh_group.param("expand", expand_, 0.1f);
00448
00449 std::string point_vel_mode;
00450 nh_group.param("point_vel_mode", point_vel_mode, std::string("prev"));
00451 std::transform(point_vel_mode.begin(), point_vel_mode.end(), point_vel_mode.begin(), ::tolower);
00452 if (point_vel_mode.compare("prev") == 0)
00453 point_vel_ = VEL_PREV;
00454 else if (point_vel_mode.compare("next") == 0)
00455 point_vel_ = VEL_NEXT;
00456 else if (point_vel_mode.compare("avg") == 0)
00457 point_vel_ = VEL_AVG;
00458 else
00459 ROS_ERROR("point_vel_mode must be prev/next/avg");
00460
00461 ROS_INFO("Resolution: %d", resolution_);
00462 Astar::Vec p;
00463 for (p[0] = 0; p[0] < resolution_ * 2; p[0]++)
00464 {
00465 for (p[1] = 0; p[1] < resolution_ * 2; p[1]++)
00466 {
00467 Astar::Vecf pf;
00468 grid2Metric(p, pf);
00469
00470 if (links_[0].isCollide(links_[1], pf[0], pf[1]))
00471 cm_[p] = 100;
00472
00473
00474 else
00475 cm_[p] = 0;
00476 }
00477 }
00478 for (p[0] = 0; p[0] < resolution_ * 2; p[0]++)
00479 {
00480 for (p[1] = 0; p[1] < resolution_ * 2; p[1]++)
00481 {
00482 if (cm_[p] != 100)
00483 continue;
00484
00485 Astar::Vec d;
00486 int range = lroundf(expand_ * resolution_ / (2.0 * M_PI));
00487 for (d[0] = -range; d[0] <= range; d[0]++)
00488 {
00489 for (d[1] = -range; d[1] <= range; d[1]++)
00490 {
00491 Astar::Vec p2 = p + d;
00492 if ((unsigned int)p2[0] >= (unsigned int)resolution_ * 2 ||
00493 (unsigned int)p2[1] >= (unsigned int)resolution_ * 2)
00494 continue;
00495 int dist = std::max(abs(d[0]), abs(d[1]));
00496 int c = floorf(100.0 * (range - dist) / range);
00497 if (cm_[p2] < c)
00498 cm_[p2] = c;
00499 }
00500 }
00501 }
00502 }
00503
00504 int range;
00505 nh_group.param("range", range, 8);
00506 for (p[0] = -range; p[0] <= range; p[0]++)
00507 {
00508 for (p[1] = -range; p[1] <= range; p[1]++)
00509 {
00510 search_list_.push_back(p);
00511 }
00512 }
00513
00514 int num_threads;
00515 nh_group.param("num_threads", num_threads, 1);
00516 omp_set_num_threads(num_threads);
00517
00518 has_start_ = has_goal_ = true;
00519 }
00520
00521 private:
00522 void grid2Metric(
00523 const int t0, const int t1,
00524 float& gt0, float& gt1)
00525 {
00526 gt0 = (t0 - resolution_) * 2.0 * M_PI / static_cast<float>(resolution_);
00527 gt1 = (t1 - resolution_) * 2.0 * M_PI / static_cast<float>(resolution_);
00528 }
00529 void metric2Grid(
00530 int& t0, int& t1,
00531 const float gt0, const float gt1)
00532 {
00533 t0 = lroundf(gt0 * resolution_ / (2.0 * M_PI)) + resolution_;
00534 t1 = lroundf(gt1 * resolution_ / (2.0 * M_PI)) + resolution_;
00535 }
00536 void grid2Metric(
00537 const Astar::Vec t,
00538 Astar::Vecf& gt)
00539 {
00540 grid2Metric(t[0], t[1], gt[0], gt[1]);
00541 }
00542 void metric2Grid(
00543 Astar::Vec& t,
00544 const Astar::Vecf gt)
00545 {
00546 metric2Grid(t[0], t[1], gt[0], gt[1]);
00547 }
00548 bool makePlan(const Astar::Vecf sg, const Astar::Vecf eg, std::list<Astar::Vecf>& path)
00549 {
00550 Astar::Vec s, e;
00551 metric2Grid(s, sg);
00552 metric2Grid(e, eg);
00553 ROS_INFO("Planning from (%d, %d) to (%d, %d)",
00554 s[0], s[1], e[0], e[1]);
00555
00556 if (cm_[s] == 100)
00557 {
00558 ROS_WARN("Path plan failed (current status is in collision)");
00559 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
00560 return false;
00561 }
00562 if (cm_[e] == 100)
00563 {
00564 ROS_WARN("Path plan failed (goal status is in collision)");
00565 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
00566 return false;
00567 }
00568 Astar::Vec d = e - s;
00569 d.cycle(resolution_, resolution_);
00570
00571 if (cbCost(s, e, e, s) >= euclidCost(d))
00572 {
00573 path.push_back(sg);
00574 path.push_back(eg);
00575 if (s == e)
00576 {
00577 replan_prev_ = ros::Time(0);
00578 }
00579 return true;
00580 }
00581 std::list<Astar::Vec> path_grid;
00582
00583 float cancel = FLT_MAX;
00584 if (replan_interval_ >= ros::Duration(0))
00585 cancel = replan_interval_.toSec();
00586 if (!as_.search(s, e, path_grid,
00587 std::bind(&planner2dofSerialJointsNode::cbCost,
00588 this, std::placeholders::_1, std::placeholders::_2,
00589 std::placeholders::_3, std::placeholders::_4),
00590 std::bind(&planner2dofSerialJointsNode::cbCostEstim,
00591 this, std::placeholders::_1, std::placeholders::_2),
00592 std::bind(&planner2dofSerialJointsNode::cbSearch,
00593 this, std::placeholders::_1,
00594 std::placeholders::_2, std::placeholders::_3),
00595 std::bind(&planner2dofSerialJointsNode::cbProgress,
00596 this, std::placeholders::_1),
00597 0,
00598 cancel,
00599 true))
00600 {
00601 ROS_WARN("Path plan failed (goal unreachable)");
00602 status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
00603 return false;
00604 }
00605
00606
00607
00608
00609 bool first = false;
00610 Astar::Vec n_prev = s;
00611 path.push_back(sg);
00612 int i = 0;
00613 for (auto& n : path_grid)
00614 {
00615 if (!first)
00616 {
00617 first = true;
00618 continue;
00619 }
00620 if (i == 0)
00621 ROS_INFO(" next: %d, %d", n[0], n[1]);
00622 Astar::Vec n_diff = n - n_prev;
00623 n_diff.cycle(resolution_, resolution_);
00624 Astar::Vec n2 = n_prev + n_diff;
00625 n_prev = n2;
00626
00627 Astar::Vecf p;
00628 grid2Metric(n2, p);
00629 path.push_back(p);
00630 i++;
00631 }
00632 float prec = 2.0 * M_PI / static_cast<float>(resolution_);
00633 Astar::Vecf egp = eg;
00634 if (egp[0] < 0)
00635 egp[0] += ceilf(-egp[0] / M_PI * 2.0) * M_PI * 2.0;
00636 if (egp[1] < 0)
00637 egp[1] += ceilf(-egp[1] / M_PI * 2.0) * M_PI * 2.0;
00638 path.back()[0] += fmod(egp[0] + prec / 2.0, prec) - prec / 2.0;
00639 path.back()[1] += fmod(egp[1] + prec / 2.0, prec) - prec / 2.0;
00640
00641 if (debug_aa_)
00642 {
00643 Astar::Vec p;
00644 for (p[0] = resolution_ / 2; p[0] < resolution_ * 3 / 2; p[0]++)
00645 {
00646 for (p[1] = resolution_ / 2; p[1] < resolution_ * 3 / 2; p[1]++)
00647 {
00648 bool found = false;
00649 for (auto& g : path_grid)
00650 {
00651 if (g == p)
00652 found = true;
00653 }
00654 if (p == s)
00655 printf("\033[31ms\033[0m");
00656 else if (p == e)
00657 printf("\033[31me\033[0m");
00658 else if (found)
00659 printf("\033[34m*\033[0m");
00660 else
00661 printf("%d", cm_[p] / 11);
00662 }
00663 printf("\n");
00664 }
00665 printf("\n");
00666 }
00667
00668 return true;
00669 }
00670 std::vector<Astar::Vec>& cbSearch(
00671 const Astar::Vec& p,
00672 const Astar::Vec& s, const Astar::Vec& e)
00673 {
00674 return search_list_;
00675 }
00676 bool cbProgress(const std::list<Astar::Vec>& path_grid)
00677 {
00678 return false;
00679 }
00680 float cbCostEstim(const Astar::Vec& s, const Astar::Vec& e)
00681 {
00682 const Astar::Vec d = e - s;
00683 return euclidCost(d);
00684 }
00685 float cbCost(const Astar::Vec& s, Astar::Vec& e,
00686 const Astar::Vec& v_goal,
00687 const Astar::Vec& v_start)
00688 {
00689 if ((unsigned int)e[0] >= (unsigned int)resolution_ * 2 ||
00690 (unsigned int)e[1] >= (unsigned int)resolution_ * 2)
00691 return -1;
00692 Astar::Vec d = e - s;
00693 d.cycle(resolution_, resolution_);
00694
00695 float cost = euclidCost(d);
00696
00697 float distf = hypotf(static_cast<float>(d[0]), static_cast<float>(d[1]));
00698 float v[2], dp[2];
00699 int sum = 0;
00700 const int dist = distf;
00701 distf /= dist;
00702 v[0] = s[0];
00703 v[1] = s[1];
00704 dp[0] = static_cast<float>(d[0]) / dist;
00705 dp[1] = static_cast<float>(d[1]) / dist;
00706 Astar::Vec pos;
00707 for (int i = 0; i < dist; i++)
00708 {
00709 pos[0] = lroundf(v[0]);
00710 pos[1] = lroundf(v[1]);
00711 pos.cycleUnsigned(resolution_, resolution_);
00712 const auto c = cm_[pos];
00713 if (c > 99)
00714 return -1;
00715 sum += c;
00716 v[0] += dp[0];
00717 v[1] += dp[1];
00718 }
00719 cost += sum * weight_cost_ / 100.0;
00720 return cost;
00721 }
00722 };
00723
00724 int main(int argc, char* argv[])
00725 {
00726 ros::init(argc, argv, "planner_2dof_serial_joints");
00727 ros::NodeHandle pnh("~");
00728
00729 std::vector<std::shared_ptr<planner2dofSerialJointsNode>> jys;
00730 int n;
00731 pnh.param("num_groups", n, 1);
00732 for (int i = 0; i < n; i++)
00733 {
00734 std::string name;
00735 pnh.param("group" + std::to_string(i) + "_name",
00736 name, std::string("group") + std::to_string(i));
00737 std::shared_ptr<planner2dofSerialJointsNode> jy;
00738
00739 jy.reset(new planner2dofSerialJointsNode(name));
00740 jys.push_back(jy);
00741 }
00742
00743 ros::spin();
00744
00745 return 0;
00746 }