Marker6DOF(): show_6dof_circle_(true) {
ros::NodeHandle nh, pnh("~");
pnh.param("frame_id", frame_id_, std::string("/map"));
pnh.param("object_type", object_type_, std::string("sphere"));
pnh.param("object_x", object_x_, 1.0);
pnh.param("object_y", object_y_, 1.0);
pnh.param("object_z", object_z_, 1.0);
pnh.param("object_r", object_r_, 1.0);
pnh.param("object_g", object_g_, 1.0);
pnh.param("object_b", object_b_, 1.0);
pnh.param("object_a", object_a_, 1.0);
pnh.param("line_width", line_width_, 0.007);
latest_pose_.header.frame_id = frame_id_;
latest_pose_.pose.orientation.w = 1.0;
pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("pose", 1);
pose_stamped_sub_ = pnh.subscribe("move_marker", 1, &Marker6DOF::moveMarkerCB, this);
circle_menu_entry_
= menu_handler_.insert("Toggle 6DOF Circle",
boost::bind(&Marker6DOF::menuFeedbackCB, this, _1));
menu_handler_.setCheckState(circle_menu_entry_,
interactive_markers::MenuHandler::CHECKED);
server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
initializeInteractiveMarker();
}
void FootstepMarker::updateInitialFootstep() {
//ROS_INFO("updateInitialFootstep");
try {
if (!use_initial_footstep_tf_) {
return;
}
tf::StampedTransform lfoot_transform, rfoot_transform;
tf_listener_->lookupTransform(marker_frame_id_, lfoot_frame_id_, ros::Time(0.0), lfoot_transform);
tf_listener_->lookupTransform(marker_frame_id_, rfoot_frame_id_, ros::Time(0.0), rfoot_transform);
// apply offset
// convert like tf -> eigen -> msg
Eigen::Affine3d le, re;
tf::transformTFToEigen(lfoot_transform * lleg_offset_, le); // tf -> eigen
tf::poseEigenToMsg(le, lleg_initial_pose_); // eigen -> msg
tf::transformTFToEigen(rfoot_transform * rleg_offset_, re); // tf -> eigen
tf::poseEigenToMsg(re, rleg_initial_pose_); // eigen -> msg
// we need to move the marker
initializeInteractiveMarker();
}
catch (tf2::TransformException& e) {
ROS_ERROR("Failed to lookup transformation: %s", e.what());
}
}
Marker6DOF() {
ros::NodeHandle nh, pnh("~");
pnh.param("frame_id", frame_id_, std::string("/map"));
pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("pose", 1);
pose_stamped_sub_ = pnh.subscribe("move_marker", 1, &Marker6DOF::moveMarkerCB, this);
server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
initializeInteractiveMarker();
}
void menuFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
show_6dof_circle_ = !show_6dof_circle_;
if (show_6dof_circle_) {
menu_handler_.setCheckState(circle_menu_entry_,
interactive_markers::MenuHandler::CHECKED);
}
else {
menu_handler_.setCheckState(circle_menu_entry_,
interactive_markers::MenuHandler::UNCHECKED);
}
initializeInteractiveMarker(); // ok...?
}
FootstepMarker::FootstepMarker():
ac_("footstep_planner", true), ac_exec_("footstep_controller", true),
plan_run_(false), lleg_first_(true) {
// read parameters
tf_listener_.reset(new tf::TransformListener);
ros::NodeHandle pnh("~");
ros::NodeHandle nh;
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&FootstepMarker::configCallback, this, _1, _2);
srv_->setCallback (f);
pnh.param("foot_size_x", foot_size_x_, 0.247);
pnh.param("foot_size_y", foot_size_y_, 0.135);
pnh.param("foot_size_z", foot_size_z_, 0.01);
pnh.param("lfoot_frame_id", lfoot_frame_id_, std::string("lfsensor"));
pnh.param("rfoot_frame_id", rfoot_frame_id_, std::string("rfsensor"));
pnh.param("show_6dof_control", show_6dof_control_, true);
// pnh.param("use_projection_service", use_projection_service_, false);
// pnh.param("use_projection_topic", use_projection_topic_, false);
pnh.param("always_planning", always_planning_, true);
// if (use_projection_topic_) {
project_footprint_pub_ = pnh.advertise<jsk_interactive_marker::SnapFootPrintInput>("project_footprint", 1);
// }
// read lfoot_offset
readPoseParam(pnh, "lfoot_offset", lleg_offset_);
readPoseParam(pnh, "rfoot_offset", rleg_offset_);
pnh.param("footstep_margin", footstep_margin_, 0.2);
pnh.param("use_footstep_planner", use_footstep_planner_, true);
pnh.param("use_footstep_controller", use_footstep_controller_, true);
pnh.param("use_initial_footstep_tf", use_initial_footstep_tf_, true);
pnh.param("wait_snapit_server", wait_snapit_server_, false);
bool nowait = true;
pnh.param("no_wait", nowait, true);
pnh.param("frame_id", marker_frame_id_, std::string("/map"));
footstep_pub_ = nh.advertise<jsk_footstep_msgs::FootstepArray>("footstep_from_marker", 1);
snapit_client_ = nh.serviceClient<jsk_pcl_ros::CallSnapIt>("snapit");
snapped_pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("snapped_pose", 1);
current_pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("current_pose", 1);
estimate_occlusion_client_ = nh.serviceClient<std_srvs::Empty>("require_estimation");
if (!nowait && wait_snapit_server_) {
snapit_client_.waitForExistence();
}
if (pnh.getParam("initial_reference_frame", initial_reference_frame_)) {
use_initial_reference_ = true;
JSK_ROS_INFO_STREAM("initial_reference_frame: " << initial_reference_frame_);
}
else {
use_initial_reference_ = false;
JSK_ROS_INFO("initial_reference_frame is not specified ");
}
server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
// menu_handler_.insert( "Snap Legs",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
// menu_handler_.insert( "Reset Legs",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Look Ground",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Execute the Plan",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Force to replan",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
// menu_handler_.insert( "Estimate occlusion",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Cancel Walk",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Toggle 6dof marker",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
// menu_handler_.insert( "Resume Footstep",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("Straight Heuristic",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("Stepcost Heuristic**",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("LLeg First",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("RLeg First",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
marker_pose_.header.frame_id = marker_frame_id_;
marker_pose_.header.stamp = ros::Time::now();
marker_pose_.pose.orientation.w = 1.0;
resetLegPoses();
// initialize lleg_initial_pose, rleg_initial_pose
lleg_initial_pose_.position.y = footstep_margin_ / 2.0;
lleg_initial_pose_.orientation.w = 1.0;
rleg_initial_pose_.position.y = - footstep_margin_ / 2.0;
rleg_initial_pose_.orientation.w = 1.0;
if (use_initial_reference_) {
while (ros::ok()) {
try {
if (!tf_listener_->waitForTransform(marker_frame_id_, initial_reference_frame_,
ros::Time(0.0), ros::Duration(10.0))) {
ROS_INFO_THROTTLE(1.0,
"waiting for transform %s => %s", marker_frame_id_.c_str(),
initial_reference_frame_.c_str());
continue;
}
ROS_INFO("resolved transform %s => %s", marker_frame_id_.c_str(),
initial_reference_frame_.c_str());
tf::StampedTransform transform;
tf_listener_->lookupTransform(marker_frame_id_, initial_reference_frame_,
ros::Time(0), transform);
marker_pose_.pose.position.x = transform.getOrigin().x();
marker_pose_.pose.position.y = transform.getOrigin().y();
marker_pose_.pose.position.z = transform.getOrigin().z();
marker_pose_.pose.orientation.x = transform.getRotation().x();
marker_pose_.pose.orientation.y = transform.getRotation().y();
marker_pose_.pose.orientation.z = transform.getRotation().z();
marker_pose_.pose.orientation.w = transform.getRotation().w();
break;
}
catch (tf2::TransformException& e) {
ROS_ERROR("Failed to lookup transformation: %s", e.what());
}
}
}
initializeInteractiveMarker();
if (use_footstep_planner_) {
ROS_INFO("waiting planner server...");
ac_.waitForServer();
ROS_INFO("found planner server...");
}
if (use_footstep_controller_) {
ROS_INFO("waiting controller server...");
ac_exec_.waitForServer();
ROS_INFO("found controller server...");
}
move_marker_sub_ = nh.subscribe("move_marker", 1, &FootstepMarker::moveMarkerCB, this);
menu_command_sub_ = nh.subscribe("menu_command", 1, &FootstepMarker::menuCommandCB, this);
exec_sub_ = pnh.subscribe("execute", 1, &FootstepMarker::executeCB, this);
resume_sub_ = pnh.subscribe("resume", 1, &FootstepMarker::resumeCB, this);
plan_if_possible_srv_ = pnh.advertiseService("force_to_replan", &FootstepMarker::forceToReplan, this);
if (use_initial_footstep_tf_) {
// waiting TF
while (ros::ok()) {
try {
if (tf_listener_->waitForTransform(lfoot_frame_id_, marker_frame_id_,
ros::Time(0.0), ros::Duration(10.0))
&& tf_listener_->waitForTransform(rfoot_frame_id_, marker_frame_id_,
ros::Time(0.0), ros::Duration(10.0))) {
break;
}
ROS_INFO("waiting for transform {%s, %s} => %s", lfoot_frame_id_.c_str(),
rfoot_frame_id_.c_str(), marker_frame_id_.c_str());
}
catch (tf2::TransformException& e) {
ROS_ERROR("Failed to lookup transformation: %s", e.what());
}
}
ROS_INFO("resolved transform {%s, %s} => %s", lfoot_frame_id_.c_str(),
rfoot_frame_id_.c_str(), marker_frame_id_.c_str());
}
// if (use_projection_topic_) {
projection_sub_ = pnh.subscribe("projected_pose", 1,
&FootstepMarker::projectionCallback, this);
// }
}
