footstep_projection_demo.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #include <jsk_footstep_msgs/FootstepArray.h>
00036 #include "jsk_footstep_planner/footstep_state.h"
00037 #include <interactive_markers/tools.h>
00038 #include <interactive_markers/interactive_marker_server.h>
00039 #include <jsk_recognition_utils/pcl_conversion_util.h>
00040 
00041 //#include <jsk_interactive_marker/interactive_marker_helpers.h>
00042 using namespace jsk_footstep_planner;
00043 
00044 // globals
00045 ros::Publisher pub_footstep;
00046 ros::Publisher pub_projected_footstep;
00047 ros::Publisher pub_cloud;
00048 FootstepState::Ptr original_footstep;
00049 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
00050 pcl::KdTreeFLANN<pcl::PointNormal> tree;
00051 pcl::PointCloud<pcl::PointNormal>::Ptr cloud2d;
00052 pcl::search::Octree<pcl::PointNormal> tree2d(0.1);
00053 ANNGrid::Ptr grid_search;
00054 FootstepParameters parameters;
00055 
00056 jsk_footstep_msgs::FootstepArray footstepToFootstepArray(
00057   jsk_footstep_msgs::Footstep msg)
00058 {
00059   jsk_footstep_msgs::FootstepArray array_msg;
00060   array_msg.header.frame_id = "odom";
00061   array_msg.header.stamp = ros::Time::now();
00062   array_msg.footsteps.push_back(msg);
00063   return array_msg;
00064 }
00065 
00066 void processFeedback(
00067     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00068 {
00069   Eigen::Affine3f new_pose;
00070   tf::poseMsgToEigen(feedback->pose, new_pose);
00071   original_footstep->setPose(new_pose);
00072   jsk_footstep_msgs::FootstepArray msg = footstepToFootstepArray(*(original_footstep->toROSMsg()));
00073   pub_footstep.publish(msg);
00074   parameters.plane_estimation_outlier_threshold = 0.05;
00075   parameters.plane_estimation_max_iterations    = 100;
00076   parameters.plane_estimation_min_inliers       = 10;
00077   unsigned int error_state;
00078   FootstepState::Ptr projected_footstep = original_footstep->projectToCloud(
00079     tree,
00080     cloud,
00081     grid_search,
00082     tree2d,
00083     cloud2d,
00084     Eigen::Vector3f(0, 0, 1),
00085     error_state,
00086     parameters);
00087   if (projected_footstep) {
00088     jsk_footstep_msgs::FootstepArray msg2 = footstepToFootstepArray(*(projected_footstep->toROSMsg()));
00089     pub_projected_footstep.publish(msg2);
00090   }
00091   else {
00092     ROS_ERROR("error state: %u" , error_state);
00093   }
00094 }
00095 
00096 pcl::PointCloud<pcl::PointNormal>::Ptr
00097 generateCloud()
00098 {
00099   pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
00100   for (double y = -0.5; y < 0.5; y = y + 0.01) {
00101     for (double x = 0.0; x < 0.5; x = x + 0.01) {
00102       pcl::PointNormal p;
00103       p.x = x;
00104       p.y = y;
00105       gen_cloud->points.push_back(p);
00106     }
00107     for (double x = 0.5; x < 1.0; x = x + 0.01) {
00108       pcl::PointNormal p;
00109       p.x = x;
00110       p.y = y;
00111       p.z = x - 0.5;
00112       gen_cloud->points.push_back(p);
00113     }
00114     for (double x = 1.0; x < 1.5; x = x + 0.01) {
00115       pcl::PointNormal p;
00116       p.x = x;
00117       p.y = y;
00118       p.z = 0.5;
00119       gen_cloud->points.push_back(p);
00120     }
00121     for (double x = 1.5; x < 2.0; x = x + 0.01) {
00122       pcl::PointNormal p;
00123       p.x = x;
00124       p.y = y;
00125       p.z = -x + 2.0;
00126       gen_cloud->points.push_back(p);
00127     }
00128     for (double x = 2.0; x < M_PI; x = x + 0.01) {
00129       pcl::PointNormal p;
00130       p.x = x;
00131       p.y = y;
00132       gen_cloud->points.push_back(p);
00133     }
00134     for (double x = M_PI; x < 2.0 * M_PI; x = x + 0.01) {
00135       pcl::PointNormal p;
00136       p.x = x;
00137       p.y = y;
00138       p.z = std::abs(sin(2.0 * x));
00139       gen_cloud->points.push_back(p);
00140     }
00141 
00142   }
00143   return gen_cloud;
00144 }
00145 
00146 int main(int argc, char** argv)
00147 {
00148   ros::init(argc, argv, "footstep_projection_demo");
00149   ros::NodeHandle nh("~");
00150   pub_footstep
00151     = nh.advertise<jsk_footstep_msgs::FootstepArray>("original", 1);
00152   pub_projected_footstep
00153     = nh.advertise<jsk_footstep_msgs::FootstepArray>("projected", 1);
00154   pub_cloud
00155     = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1, true);
00156   
00157   // generate pointcloud
00158   cloud = generateCloud();
00159   cloud2d.reset(new pcl::PointCloud<pcl::PointNormal>);
00160   tree.setInputCloud(cloud);
00161   grid_search.reset(new ANNGrid(0.1));
00162   grid_search->build(*cloud);
00163   pcl::ProjectInliers<pcl::PointNormal> proj;
00164   pcl::ModelCoefficients::Ptr coef (new pcl::ModelCoefficients);
00165   
00166   coef->values.resize(4);
00167   coef->values[2] = 1.0;
00168   proj.setInputCloud(cloud);
00169   proj.setModelCoefficients(coef);
00170   proj.filter(*cloud2d);
00171   tree2d.setInputCloud(cloud2d);
00172   sensor_msgs::PointCloud2 ros_cloud;
00173   pcl::toROSMsg(*cloud, ros_cloud);
00174   ros_cloud.header.frame_id = "odom";
00175   ros_cloud.header.stamp = ros::Time::now();
00176   pub_cloud.publish(ros_cloud);
00177   original_footstep.reset(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00178                                             Eigen::Affine3f::Identity(),
00179                                             Eigen::Vector3f(0.2, 0.1, 0.00001),
00180                                             Eigen::Vector3f(0.05, 0.05, 0.8)));
00181   jsk_footstep_msgs::FootstepArray msg = footstepToFootstepArray(*(original_footstep->toROSMsg()));
00182   pub_footstep.publish(msg);
00183   interactive_markers::InteractiveMarkerServer server("footstep_projection_demo");
00184   visualization_msgs::InteractiveMarker int_marker;
00185   int_marker.header.frame_id = "/odom";
00186   int_marker.header.stamp=ros::Time::now();
00187   int_marker.name = "footstep marker";
00188 
00189   visualization_msgs::InteractiveMarkerControl control;
00190 
00191   control.orientation.w = 1;
00192   control.orientation.x = 1;
00193   control.orientation.y = 0;
00194   control.orientation.z = 0;
00195   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00196   int_marker.controls.push_back(control);
00197   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00198   int_marker.controls.push_back(control);
00199 
00200   control.orientation.w = 1;
00201   control.orientation.x = 0;
00202   control.orientation.y = 1;
00203   control.orientation.z = 0;
00204   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00205   int_marker.controls.push_back(control);
00206   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00207   int_marker.controls.push_back(control);
00208 
00209   control.orientation.w = 1;
00210   control.orientation.x = 0;
00211   control.orientation.y = 0;
00212   control.orientation.z = 1;
00213   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00214   int_marker.controls.push_back(control);
00215   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00216   int_marker.controls.push_back(control);
00217   
00218   server.insert(int_marker, &processFeedback);
00219   server.applyChanges();
00220   ros::spin();
00221 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:28