footstep_projection_demo.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 #include <jsk_footstep_msgs/FootstepArray.h>
40 
41 //#include <jsk_interactive_marker/interactive_marker_helpers.h>
42 using namespace jsk_footstep_planner;
43 
44 // globals
49 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
50 pcl::KdTreeFLANN<pcl::PointNormal> tree;
51 pcl::PointCloud<pcl::PointNormal>::Ptr cloud2d;
52 pcl::search::Octree<pcl::PointNormal> tree2d(0.1);
55 
56 jsk_footstep_msgs::FootstepArray footstepToFootstepArray(
57  jsk_footstep_msgs::Footstep msg)
58 {
59  jsk_footstep_msgs::FootstepArray array_msg;
60  array_msg.header.frame_id = "odom";
61  array_msg.header.stamp = ros::Time::now();
62  array_msg.footsteps.push_back(msg);
63  return array_msg;
64 }
65 
67  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
68 {
69  Eigen::Affine3f new_pose;
70  tf::poseMsgToEigen(feedback->pose, new_pose);
71  original_footstep->setPose(new_pose);
72  jsk_footstep_msgs::FootstepArray msg = footstepToFootstepArray(*(original_footstep->toROSMsg()));
73  pub_footstep.publish(msg);
74  parameters.plane_estimation_outlier_threshold = 0.05;
75  parameters.plane_estimation_max_iterations = 100;
76  parameters.plane_estimation_min_inliers = 10;
77  unsigned int error_state;
78  FootstepState::Ptr projected_footstep = original_footstep->projectToCloud(
79  tree,
80  cloud,
81  grid_search,
82  tree2d,
83  cloud2d,
84  Eigen::Vector3f(0, 0, 1),
85  error_state,
86  parameters);
87  if (projected_footstep) {
88  jsk_footstep_msgs::FootstepArray msg2 = footstepToFootstepArray(*(projected_footstep->toROSMsg()));
89  pub_projected_footstep.publish(msg2);
90  }
91  else {
92  ROS_ERROR("error state: %u" , error_state);
93  }
94 }
95 
96 pcl::PointCloud<pcl::PointNormal>::Ptr
98 {
99  pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
100  for (double y = -0.5; y < 0.5; y = y + 0.01) {
101  for (double x = 0.0; x < 0.5; x = x + 0.01) {
102  pcl::PointNormal p;
103  p.x = x;
104  p.y = y;
105  gen_cloud->points.push_back(p);
106  }
107  for (double x = 0.5; x < 1.0; x = x + 0.01) {
108  pcl::PointNormal p;
109  p.x = x;
110  p.y = y;
111  p.z = x - 0.5;
112  gen_cloud->points.push_back(p);
113  }
114  for (double x = 1.0; x < 1.5; x = x + 0.01) {
115  pcl::PointNormal p;
116  p.x = x;
117  p.y = y;
118  p.z = 0.5;
119  gen_cloud->points.push_back(p);
120  }
121  for (double x = 1.5; x < 2.0; x = x + 0.01) {
122  pcl::PointNormal p;
123  p.x = x;
124  p.y = y;
125  p.z = -x + 2.0;
126  gen_cloud->points.push_back(p);
127  }
128  for (double x = 2.0; x < M_PI; x = x + 0.01) {
129  pcl::PointNormal p;
130  p.x = x;
131  p.y = y;
132  gen_cloud->points.push_back(p);
133  }
134  for (double x = M_PI; x < 2.0 * M_PI; x = x + 0.01) {
135  pcl::PointNormal p;
136  p.x = x;
137  p.y = y;
138  p.z = std::abs(sin(2.0 * x));
139  gen_cloud->points.push_back(p);
140  }
141 
142  }
143  return gen_cloud;
144 }
145 
146 int main(int argc, char** argv)
147 {
148  ros::init(argc, argv, "footstep_projection_demo");
149  ros::NodeHandle nh("~");
150  pub_footstep
151  = nh.advertise<jsk_footstep_msgs::FootstepArray>("original", 1);
152  pub_projected_footstep
153  = nh.advertise<jsk_footstep_msgs::FootstepArray>("projected", 1);
154  pub_cloud
155  = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1, true);
156 
157  // generate pointcloud
158  cloud = generateCloud();
159  cloud2d.reset(new pcl::PointCloud<pcl::PointNormal>);
160  tree.setInputCloud(cloud);
161  grid_search.reset(new ANNGrid(0.1));
162  grid_search->build(*cloud);
163  pcl::ProjectInliers<pcl::PointNormal> proj;
164  pcl::ModelCoefficients::Ptr coef (new pcl::ModelCoefficients);
165 
166  coef->values.resize(4);
167  coef->values[2] = 1.0;
168  proj.setInputCloud(cloud);
169  proj.setModelCoefficients(coef);
170  proj.filter(*cloud2d);
171  tree2d.setInputCloud(cloud2d);
172  sensor_msgs::PointCloud2 ros_cloud;
173  pcl::toROSMsg(*cloud, ros_cloud);
174  ros_cloud.header.frame_id = "odom";
175  ros_cloud.header.stamp = ros::Time::now();
176  pub_cloud.publish(ros_cloud);
177  original_footstep.reset(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
178  Eigen::Affine3f::Identity(),
179  Eigen::Vector3f(0.2, 0.1, 0.00001),
180  Eigen::Vector3f(0.05, 0.05, 0.8)));
181  jsk_footstep_msgs::FootstepArray msg = footstepToFootstepArray(*(original_footstep->toROSMsg()));
182  pub_footstep.publish(msg);
183  interactive_markers::InteractiveMarkerServer server("footstep_projection_demo");
184  visualization_msgs::InteractiveMarker int_marker;
185  int_marker.header.frame_id = "/odom";
186  int_marker.header.stamp=ros::Time::now();
187  int_marker.name = "footstep marker";
188 
189  visualization_msgs::InteractiveMarkerControl control;
190 
191  control.orientation.w = 1;
192  control.orientation.x = 1;
193  control.orientation.y = 0;
194  control.orientation.z = 0;
195  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
196  int_marker.controls.push_back(control);
197  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
198  int_marker.controls.push_back(control);
199 
200  control.orientation.w = 1;
201  control.orientation.x = 0;
202  control.orientation.y = 1;
203  control.orientation.z = 0;
204  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
205  int_marker.controls.push_back(control);
206  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
207  int_marker.controls.push_back(control);
208 
209  control.orientation.w = 1;
210  control.orientation.x = 0;
211  control.orientation.y = 0;
212  control.orientation.z = 1;
213  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
214  int_marker.controls.push_back(control);
215  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
216  int_marker.controls.push_back(control);
217 
218  server.insert(int_marker, &processFeedback);
219  server.applyChanges();
220  ros::spin();
221 }
msg
ANNGrid is a class to provide approximate near neighbors search based on 2.5-D representation. All the z values of pointcloud is ignored and it sorted as 2-D array.
Definition: ann_grid.h:93
jsk_footstep_msgs::FootstepArray footstepToFootstepArray(jsk_footstep_msgs::Footstep msg)
ANNGrid::Ptr grid_search
void publish(const boost::shared_ptr< M > &message) const
pcl::PointCloud< pcl::PointNormal >::Ptr cloud2d
FootstepParameters parameters
double sin()
pcl::PointCloud< pcl::PointNormal >::Ptr generateCloud()
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
server
pcl::KdTreeFLANN< pcl::PointNormal > tree
double y
ROSCPP_DECL void spin(Spinner &spinner)
#define M_PI
ros::Publisher pub_footstep
ros::Publisher pub_projected_footstep
FootstepState::Ptr original_footstep
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_cloud
coef
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
pcl::search::Octree< pcl::PointNormal > tree2d(0.1)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
static Time now()
double x
#define ROS_ERROR(...)


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri May 14 2021 02:51:52