40 #include <pcl/common/angles.h> 42 #include <boost/format.hpp> 50 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
51 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
53 srv_->setCallback (f);
57 "close_list", 1,
true);
59 "open_list", 1,
true);
70 std::vector<double> lleg_footstep_offset, rleg_footstep_offset;
73 - lleg_footstep_offset[1],
74 - lleg_footstep_offset[2]);
80 - rleg_footstep_offset[1],
81 - rleg_footstep_offset[2]);
97 std::vector<double> collision_bbox_size, collision_bbox_offset;
105 collision_bbox_offset[1],
106 collision_bbox_offset[2]);
112 const sensor_msgs::PointCloud2::ConstPtr& msg)
124 const sensor_msgs::PointCloud2::ConstPtr& msg)
127 ROS_DEBUG(
"pointcloud model is updated");
137 const Eigen::Affine3f& center_pose,
138 const Eigen::Affine3f& left_pose_trans,
139 const Eigen::Affine3f& right_pose_trans,
140 geometry_msgs::Pose& pose)
148 Eigen::Affine3f left_pose = center_pose * left_pose_trans;
149 Eigen::Affine3f right_pose = center_pose * right_pose_trans;
151 jsk_footstep_msgs::Footstep::LEFT,
156 jsk_footstep_msgs::Footstep::RIGHT,
162 if (!projected_left || !projected_right) {
165 Eigen::Affine3f projected_left_pose = projected_left->getPose();
166 Eigen::Affine3f projected_right_pose = projected_right->getPose();
167 Eigen::Quaternionf
rot = Eigen::Quaternionf(projected_left_pose.rotation()).slerp(
168 0.5, Eigen::Quaternionf(projected_right_pose.rotation()));
169 Eigen::Vector3f
pos = (Eigen::Vector3f(projected_right_pose.translation()) +
170 Eigen::Vector3f(projected_left_pose.translation())) / 2.0;
171 Eigen::Affine3f mid = Eigen::Translation3f(pos) * rot;
177 jsk_interactive_marker::SnapFootPrint::Request& req,
178 jsk_interactive_marker::SnapFootPrint::Response& res)
185 ROS_ERROR(
"No pointcloud model is yet available");
187 "No pointcloud model is yet available",
191 Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
192 std::vector<Eigen::Affine3f> center_poses;
196 const double dx = 0.05;
197 const double dy = 0.05;
198 const double dtheta = pcl::deg2rad(5.0);
199 for (
int xi = 0; xi < 3; xi++) {
200 for (
int yi = 0; yi < 3; yi++) {
201 for (
int thetai = 0; thetai < 3; thetai++) {
202 Eigen::Affine3f transppp =
affineFromXYYaw(xi * dx, yi * dy, thetai * dtheta);
203 Eigen::Affine3f transppm =
affineFromXYYaw(xi * dx, yi * dy, - thetai * dtheta);
204 Eigen::Affine3f transpmp =
affineFromXYYaw(xi * dx, - yi * dy, thetai * dtheta);
205 Eigen::Affine3f transpmm =
affineFromXYYaw(xi * dx, - yi * dy, -thetai * dtheta);
206 Eigen::Affine3f transmpp =
affineFromXYYaw(- xi * dx, yi * dy, thetai * dtheta);
207 Eigen::Affine3f transmpm =
affineFromXYYaw(- xi * dx, yi * dy, - thetai * dtheta);
208 Eigen::Affine3f transmmp =
affineFromXYYaw(- xi * dx, - yi * dy, thetai * dtheta);
209 Eigen::Affine3f transmmm =
affineFromXYYaw(- xi * dx, - yi * dy, - thetai * dtheta);
210 center_poses.push_back(center_pose * transppp);
211 center_poses.push_back(center_pose * transppm);
212 center_poses.push_back(center_pose * transpmp);
213 center_poses.push_back(center_pose * transpmm);
214 center_poses.push_back(center_pose * transmpp);
215 center_poses.push_back(center_pose * transmpm);
216 center_poses.push_back(center_pose * transmmp);
217 center_poses.push_back(center_pose * transmmm);
221 for (
size_t i = 0; i < center_poses.size(); i++) {
223 res.snapped_pose.pose)) {
225 res.snapped_pose.header = req.input_pose.header;
229 ROS_ERROR(
"Failed to project footprint");
231 "Failed to project goal",
237 jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
238 jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res)
249 jsk_footstep_planner::ProjectFootstep::Request& req,
250 jsk_footstep_planner::ProjectFootstep::Response& res)
257 ROS_ERROR(
"No pointcloud model is yet available");
271 for (std::vector<jsk_footstep_msgs::Footstep>::iterator it = req.input.footsteps.begin();
272 it != req.input.footsteps.end(); it++) {
273 if (it->offset.x == 0.0 &&
274 it->offset.y == 0.0 &&
275 it->offset.z == 0.0 ) {
276 if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
286 if(it->dimensions.x == 0 &&
287 it->dimensions.y == 0 &&
288 it->dimensions.z == 0 ) {
291 it->dimensions.z = 0.000001;
296 res.success.push_back(
true);
297 jsk_footstep_msgs::Footstep::Ptr
p;
298 if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
300 }
else if (it->leg == jsk_footstep_msgs::Footstep::RIGHT) {
303 p = projected->toROSMsg();
305 res.result.footsteps.push_back(*p);
307 res.success.push_back(
false);
308 res.result.footsteps.push_back(*it);
311 res.result.header = req.input.header;
317 jsk_interactive_marker::SnapFootPrint::Request& req,
318 jsk_interactive_marker::SnapFootPrint::Response& res)
325 ROS_ERROR(
"No pointcloud model is yet available");
327 "No pointcloud model is yet available",
331 Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
336 res.snapped_pose.pose)) {
338 res.snapped_pose.header = req.input_pose.header;
342 ROS_ERROR(
"Failed to project footprint");
344 "Failed to project goal",
350 jsk_footstep_planner::SetHeuristicPath::Request& req,
351 jsk_footstep_planner::SetHeuristicPath::Response& res)
357 std::vector<Eigen::Vector3f>
points;
358 for(
int i = 0; i < req.segments.size(); i++) {
359 Eigen::Vector3f
p(req.segments[i].x,
371 const std::string& text,
374 std_msgs::ColorRGBA ok_color;
375 ok_color.r = 0.3568627450980392;
376 ok_color.g = 0.7529411764705882;
377 ok_color.b = 0.8705882352941177;
379 std_msgs::ColorRGBA warn_color;
380 warn_color.r = 0.9411764705882353;
381 warn_color.g = 0.6784313725490196;
382 warn_color.b = 0.3058823529411765;
384 std_msgs::ColorRGBA error_color;
385 error_color.r = 0.8509803921568627;
386 error_color.g = 0.3254901960784314;
387 error_color.b = 0.30980392156862746;
389 std_msgs::ColorRGBA color;
396 else if (status ==
ERROR) {
399 jsk_rviz_plugins::OverlayText
msg;
405 msg.bg_color.a = 0.0;
406 msg.fg_color = color;
412 const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
418 if (goal->initial_footstep.footsteps.size() == 0) {
419 ROS_ERROR(
"no initial footstep is specified");
423 if (goal->goal_footstep.footsteps.size() != 2) {
424 ROS_ERROR(
"Need to specify 2 goal footsteps");
429 ROS_ERROR(
"No pointcloud model is yet available");
434 std::string goal_frame_id = goal->initial_footstep.header.frame_id;
438 ROS_ERROR(
"frame_id of goal and pointcloud do not match. goal: %s, pointcloud: %s.",
447 ROS_ERROR(
"frame_id of goal and obstacle pointcloud do not match. goal: %s, obstacle: %s.",
457 if (goal->goal_footstep.footsteps.size() != 2) {
458 ROS_ERROR(
"goal footstep should be a pair of footsteps");
462 std::vector<jsk_footstep_msgs::Footstep > goal_ros;
463 goal_ros.push_back(goal->goal_footstep.footsteps[0]);
464 goal_ros.push_back(goal->goal_footstep.footsteps[1]);
465 for (
int i = 0; i < 2; i++) {
466 if (goal_ros[i].offset.x == 0.0 &&
467 goal_ros[i].offset.y == 0.0 &&
468 goal_ros[i].offset.z == 0.0 ) {
469 if (goal_ros[i].leg == jsk_footstep_msgs::Footstep::LEFT) {
479 if(goal_ros[i].dimensions.x == 0 &&
480 goal_ros[i].dimensions.y == 0 &&
481 goal_ros[i].dimensions.z == 0 ) {
484 goal_ros[i].dimensions.z = 0.000001;
494 if (!
graph_->isSuccessable(second_goal, first_goal)) {
500 if(goal->timeout.toSec() == 0.0) {
510 jsk_footstep_msgs::Footstep start_ros = goal->initial_footstep.footsteps[0];
511 if (start_ros.offset.x == 0.0 &&
512 start_ros.offset.y == 0.0 &&
513 start_ros.offset.z == 0.0 ) {
514 if (start_ros.leg == jsk_footstep_msgs::Footstep::LEFT) {
528 graph_->setStartState(start);
530 if (!
graph_->projectStart()) {
531 ROS_ERROR(
"Failed to project start state");
533 "Failed to project start",
543 jsk_footstep_msgs::Footstep left_goal, right_goal;
544 for (
size_t i = 0; i < goal_ros.size(); i++) {
551 if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
552 graph_->setLeftGoalState(goal_state);
553 left_goal = goal_ros[i];
555 else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
556 graph_->setRightGoalState(goal_state);
557 right_goal = goal_ros[i];
566 if (!
graph_->projectGoal()) {
570 "Failed to project goal",
577 graph_->setTransitionLimit(
594 graph_->setGlobalTransitionLimit(
637 graph_->clearPerceptionDuration();
641 std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr>
path = solver.
solve(timeout);
643 double planning_duration = (end_time - start_time).toSec();
646 if (path.size() == 0) {
647 pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
651 = (boost::format(
"Failed to plan path / Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
653 %
graph_->getPerceptionDuration().toSec()
654 % (planning_duration -
graph_->getPerceptionDuration().toSec())
656 % open_list_cloud.points.size()
657 % close_list_cloud.points.size()).
str();
671 std::vector <FootstepState::Ptr> finalizeSteps;
673 path[path.size()-1]->getState(),
677 "Failed to finalize path",
683 jsk_footstep_msgs::FootstepArray ros_path;
684 ros_path.header = goal->goal_footstep.header;
685 for (
size_t i = 0; i < path.size(); i++) {
687 if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
695 if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
704 pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
710 = (boost::format(
"Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
712 %
graph_->getPerceptionDuration().toSec()
713 % (planning_duration -
graph_->getPerceptionDuration().toSec())
715 % open_list_cloud.points.size()
716 % close_list_cloud.points.size()).
str();
723 const pcl::PointCloud<pcl::PointNormal>&
cloud,
725 const std_msgs::Header& header)
727 sensor_msgs::PointCloud2 ros_cloud;
729 ros_cloud.header = header;
742 (boost::format(
"open_list: %lu\nclose list:%lu")
746 pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
800 ROS_FATAL(
"no successors are specified");
804 double default_x = 0.0;
805 double default_y = 0.0;
806 double default_theta = 0.0;
807 if (nh.
hasParam(
"default_lfoot_to_rfoot_offset")) {
808 std::vector<double> default_offset;
810 default_x = default_offset[0];
811 default_y = default_offset[1];
812 default_theta = default_offset[2];
817 nh.
param(
"successors", successors_xml, successors_xml);
820 ROS_FATAL(
"successors should be an array");
823 for (
size_t i_successors = 0; i_successors < successors_xml.
size(); i_successors++) {
825 successor_xml = successors_xml[i_successors];
827 ROS_FATAL(
"element of successors should be an dictionary");
843 theta += default_theta;
848 Eigen::Affine3f successor =
859 if ((default_x != 0.0) || (default_y != 0.0) || (default_theta != 0.0)) {
860 ROS_INFO(
"default_offset: #f(%f %f %f)", default_x, default_y, default_theta);
865 ROS_INFO(
"left_leg_offset: #f(%f %f %f)",
873 ROS_INFO(
"right_leg_offset: #f(%f %f %f)",
881 pcl::getEulerAngles(
successors_[i], roll, pitch, yaw);
882 ROS_INFO(
"successor_%2.2d: (make-coords :pos (scale 1000 #f(%f %f 0)) :rpy (list %f 0 0))", i, tr[0], tr[1], yaw);
890 bool need_to_rebuild_graph =
false;
893 need_to_rebuild_graph =
true;
897 need_to_rebuild_graph =
true;
901 need_to_rebuild_graph =
true;
905 need_to_rebuild_graph =
true;
909 need_to_rebuild_graph =
true;
913 need_to_rebuild_graph =
true;
966 need_to_rebuild_graph =
true;
970 if (need_to_rebuild_graph) {
1000 graph_->setHeuristicPathLine(path_line);
void publish(const boost::shared_ptr< M > &message) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL const std::string & getNamespace()
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
bool isPreemptRequested()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
bool hasMember(const std::string &name) const
#define ROS_INFO_STREAM(args)
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)