35 #include <jsk_footstep_msgs/FootstepArray.h> 40 #include <boost/random.hpp> 44 #include <jsk_rviz_plugins/OverlayText.h> 45 #include <boost/format.hpp> 49 const Eigen::Vector3f
resolution(0.05, 0.05, 0.08);
59 void plan(
const Eigen::Affine3f& goal_center,
65 goal_center * Eigen::Translation3f(0, 0.1, 0),
69 goal_center * Eigen::Translation3f(0, -0.1, 0),
73 jsk_footstep_msgs::FootstepArray ros_goal;
74 ros_goal.header.frame_id =
"odom";
76 ros_goal.footsteps.push_back(*left_goal->toROSMsg());
77 ros_goal.footsteps.push_back(*right_goal->toROSMsg());
80 graph->setGoalState(left_goal, right_goal);
88 std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr>
path = solver.
solve();
90 std::cout <<
"took " << (end_time - start_time).toSec() <<
" sec" << std::endl;
91 std::cout <<
"path: " << path.size() << std::endl;
92 jsk_footstep_msgs::FootstepArray ros_path;
93 ros_path.header.frame_id =
"odom";
95 for (
size_t i = 0; i < path.size(); i++) {
96 ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
104 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
106 Eigen::Affine3f new_pose;
111 jsk_rviz_plugins::OverlayText
text;
112 text.bg_color.a = 0.0;
113 text.fg_color.a = 1.0;
114 text.fg_color.r = 25 / 255.0;
115 text.fg_color.g = 1.0;
116 text.fg_color.b = 250 / 255.0;
123 text.text = (boost::format(
"Planning took %f sec") % (end - start).toSec()).
str();
127 int main(
int argc,
char** argv)
129 ros::init(argc, argv,
"foootstep_planning_2d");
132 pub_goal = nh.
advertise<jsk_footstep_msgs::FootstepArray>(
"goal", 1,
true);
133 pub_path = nh.
advertise<jsk_footstep_msgs::FootstepArray>(
"path", 1,
true);
134 pub_text = nh.
advertise<jsk_rviz_plugins::OverlayText>(
"text", 1,
true);
135 boost::mt19937 rng( static_cast<unsigned long>(time(0)) );
136 boost::uniform_real<> xyrange(-3.0,3.0);
137 boost::variate_generator< boost::mt19937, boost::uniform_real<> > pos_rand(rng, xyrange);
138 boost::uniform_real<> trange(0, 2 *
M_PI);
139 boost::variate_generator< boost::mt19937, boost::uniform_real<> > rot_rand(rng, trange);
144 std::vector<Eigen::Affine3f> successors;
163 Eigen::Affine3f::Identity(),
166 graph->setStartState(start);
167 graph->setBasicSuccessors(successors);
168 jsk_footstep_msgs::FootstepArray ros_start;
169 ros_start.header.frame_id =
"odom";
171 ros_start.footsteps.push_back(*start->toROSMsg());
175 visualization_msgs::InteractiveMarker int_marker;
176 int_marker.header.frame_id =
"/odom";
178 int_marker.name =
"footstep marker";
180 visualization_msgs::InteractiveMarkerControl control;
182 control.orientation.w = 1;
183 control.orientation.x = 1;
184 control.orientation.y = 0;
185 control.orientation.z = 0;
188 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
189 int_marker.controls.push_back(control);
191 control.orientation.w = 1;
192 control.orientation.x = 0;
193 control.orientation.y = 1;
194 control.orientation.z = 0;
195 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
196 int_marker.controls.push_back(control);
200 control.orientation.w = 1;
201 control.orientation.x = 0;
202 control.orientation.y = 0;
203 control.orientation.z = 1;
206 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
207 int_marker.controls.push_back(control);
void publish(const boost::shared_ptr< M > &message) const
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()