From 3a8771f97d75c42d05dddbcaeab57576945bf57a Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Mon, 3 Aug 2015 21:53:41 +0900 Subject: [PATCH] [jsk_interactive_marker/footstep_marker] Add dynamic reconfirue API to toggle projection to pointcloud --- .../jsk_interactive_marker/CMakeLists.txt | 1 + .../cfg/FootstepMarker.cfg | 20 +++ .../jsk_interactive_marker/footstep_marker.h | 10 +- .../src/footstep_marker.cpp | 137 +++--------------- 4 files changed, 50 insertions(+), 118 deletions(-) create mode 100755 jsk_interactive_markers/jsk_interactive_marker/cfg/FootstepMarker.cfg diff --git a/jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt b/jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt index 1410b5e8d..c8d3b246c 100644 --- a/jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt +++ b/jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt @@ -32,6 +32,7 @@ add_service_files(DIRECTORY srv SetHeuristic.srv) generate_dynamic_reconfigure_options( + cfg/FootstepMarker.cfg cfg/InteractivePointCloud.cfg cfg/PointCloudCropper.cfg cfg/CameraInfoPublisher.cfg diff --git a/jsk_interactive_markers/jsk_interactive_marker/cfg/FootstepMarker.cfg b/jsk_interactive_markers/jsk_interactive_marker/cfg/FootstepMarker.cfg new file mode 100755 index 000000000..33fea4207 --- /dev/null +++ b/jsk_interactive_markers/jsk_interactive_marker/cfg/FootstepMarker.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python +# set up parameters that we care about +PACKAGE = 'jsk_interactive_marker' + +try: + import imp + imp.find_module(PACKAGE) + from dynamic_reconfigure.parameter_generator_catkin import *; +except: + import roslib; roslib.load_manifest(PACKAGE) + from dynamic_reconfigure.parameter_generator import *; + +from math import pi + +gen = ParameterGenerator () +gen.add("use_projection_service", bool_t, 0, "", False) +gen.add("use_projection_topic", bool_t, 0, "", False) +gen.add("use_plane_snap", bool_t, 0, "", False) + +exit (gen.generate (PACKAGE, "jsk_interactive_marker", "FootstepMarker")) diff --git a/jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/footstep_marker.h b/jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/footstep_marker.h index b416bb062..6c0e3ec42 100644 --- a/jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/footstep_marker.h +++ b/jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/footstep_marker.h @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -57,9 +58,11 @@ #include #include #include +#include class FootstepMarker { public: + typedef jsk_interactive_marker::FootstepMarkerConfig Config; FootstepMarker(); virtual ~FootstepMarker(); void updateInitialFootstep(); @@ -76,7 +79,6 @@ class FootstepMarker { void menuCommandCB(const std_msgs::UInt8::ConstPtr& msg); void executeCB(const std_msgs::Empty::ConstPtr& msg); void resumeCB(const std_msgs::Empty::ConstPtr& msg); - void planeCB(const jsk_recognition_msgs::SimpleOccupancyGridArray::ConstPtr& grid_msg); void planDoneCB(const actionlib::SimpleClientGoalState &state, const PlanResult::ConstPtr &result); void processMenuFeedback(uint8_t id); @@ -90,9 +92,10 @@ class FootstepMarker { void planIfPossible(); void resetLegPoses(); void lookGround(); + void configCallback(Config& config, uint32_t level); boost::mutex plane_mutex_; boost::mutex plan_run_mutex_; - + boost::shared_ptr > srv_; // projection to the planes bool projectMarkerToPlane(); @@ -125,7 +128,6 @@ class FootstepMarker { ros::Subscriber menu_command_sub_; ros::Subscriber exec_sub_; ros::Subscriber resume_sub_; - ros::Subscriber grid_sub_; ros::Subscriber projection_sub_; ros::Publisher project_footprint_pub_; ros::Publisher snapped_pose_pub_; @@ -141,8 +143,8 @@ class FootstepMarker { bool show_6dof_control_; bool use_footstep_planner_; bool use_footstep_controller_; - bool use_plane_snap_; bool plan_run_; + bool use_plane_snap_; bool wait_snapit_server_; bool use_initial_footstep_tf_; bool use_initial_reference_; diff --git a/jsk_interactive_markers/jsk_interactive_marker/src/footstep_marker.cpp b/jsk_interactive_markers/jsk_interactive_marker/src/footstep_marker.cpp index 8e19dc782..e01c01966 100644 --- a/jsk_interactive_markers/jsk_interactive_marker/src/footstep_marker.cpp +++ b/jsk_interactive_markers/jsk_interactive_marker/src/footstep_marker.cpp @@ -61,25 +61,29 @@ plan_run_(false), lleg_first_(true) { tf_listener_.reset(new tf::TransformListener); ros::NodeHandle pnh("~"); ros::NodeHandle nh; + srv_ = boost::make_shared > (pnh); + typename dynamic_reconfigure::Server::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("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_) { + // if (use_projection_topic_) { project_footprint_pub_ = pnh.advertise("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_plane_snap", use_plane_snap_, 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); @@ -95,10 +99,6 @@ plan_run_(false), lleg_first_(true) { snapit_client_.waitForExistence(); } - if (use_plane_snap_) { - grid_sub_ = pnh.subscribe("grid_arrays", 1, &FootstepMarker::planeCB, this); - } - if (pnh.getParam("initial_reference_frame", initial_reference_frame_)) { use_initial_reference_ = true; JSK_ROS_INFO_STREAM("initial_reference_frame: " << initial_reference_frame_); @@ -214,10 +214,18 @@ plan_run_(false), lleg_first_(true) { 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_) { + // if (use_projection_topic_) { projection_sub_ = pnh.subscribe("projected_pose", 1, &FootstepMarker::projectionCallback, this); - } + // } +} + +void FootstepMarker::configCallback(Config& config, uint32_t level) +{ + boost::mutex::scoped_lock lock(plane_mutex_); + use_projection_topic_ = config.use_projection_topic; + use_projection_service_ = config.use_projection_service; + use_plane_snap_ = config.use_plane_snap; } // a function to read double value from XmlRpcValue. @@ -519,87 +527,6 @@ bool FootstepMarker::projectMarkerToPlane() project_footprint_pub_.publish(msg); return true; // true...? } - else { - if (latest_grids_->grids.size() == 0) { - ROS_WARN("it's not valid grids"); - return false; - } - // Convert to jsk_pcl_ros::GridPlane object - std::vector grids; - for (size_t i = 0; i < latest_grids_->grids.size(); i++) { - - tf::StampedTransform tf_transform - = jsk_pcl_ros::lookupTransformWithDuration( - tf_listener_.get(), - marker_pose_.header.frame_id, - latest_grids_->grids[i].header.frame_id, - marker_pose_.header.stamp, - ros::Duration(1.0)); - - Eigen::Affine3f transform; - tf::transformTFToEigen(tf_transform, transform); - // ROS_INFO("transform: %s -> %s: [%f, %f, %f]", - // marker_pose_.header.frame_id.c_str(), - // latest_grids_->grids[i].header.frame_id.c_str(), - // transform.translation()[0], - // transform.translation()[1], - // transform.translation()[2]); - - grids.push_back(boost::make_shared( - jsk_pcl_ros::GridPlane::fromROSMsg( - latest_grids_->grids[i], - transform))); - } - //ROS_ERROR("projecting"); - double min_distance = DBL_MAX; - geometry_msgs::PoseStamped min_pose; - Eigen::Affine3f marker_coords; - // ROS_INFO_STREAM("marker_pose: " << marker_pose_.pose); - tf::poseMsgToEigen(marker_pose_.pose, marker_coords); - Eigen::Vector3f marker_pos(marker_coords.translation()); - for (size_t i = 0; i < grids.size(); i++) { - Eigen::Affine3f projected_coords; - grids[i]->getPolygon()->projectOnPlane(marker_coords, projected_coords); - Eigen::Vector3f projected_normal = projected_coords.rotation() * Eigen::Vector3f::UnitZ(); - Eigen::Vector3f normal = grids[i]->getPolygon()->getNormal(); - Eigen::Vector3f projected_point(projected_coords.translation()); - Eigen::Quaternionf projected_rot(projected_coords.rotation()); - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] normal: [%f, %f, %f]", - // normal[0], normal[1], normal[2]); - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] projected_normal: [%f, %f, %f]", - // projected_normal[0], projected_normal[1], projected_normal[2]); - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] projected point: [%f, %f, %f]", - // projected_point[0], projected_point[1], projected_point[2]); - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] projected rot: [%f, %f, %f, %f]", - // projected_rot.x(), projected_rot.y(), projected_rot.z(), projected_rot.w()); - if (grids[i]->isOccupiedGlobal(projected_point)) { - double distance = (marker_coords.translation() - projected_point).norm(); - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] distance at %lu is %f", i, distance); - if (distance < min_distance) { - min_distance = distance; - tf::poseEigenToMsg(projected_coords, min_pose.pose); - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] z diff: %f(%f)", acos(projected_normal.dot(normal)), projected_normal.dot(normal)); - } - } - else { - // ROS_INFO("[FootstepMarker::projectMarkerToPlane] not occupied at %lu", i); - } - } - // ROS_INFO_STREAM("min_distance: " << min_distance); - if (min_distance < 0.3) { // smaller than 30cm - marker_pose_.pose = min_pose.pose; - snapped_pose_pub_.publish(min_pose); - current_pose_pub_.publish(min_pose); - server_->setPose("footstep_marker", min_pose.pose); - server_->applyChanges(); - //ROS_WARN("projected"); - return true; - } - else { - //ROS_WARN("not projected"); - return false; - } - } } void FootstepMarker::menuFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { @@ -794,15 +721,10 @@ void FootstepMarker::moveMarkerCB(const geometry_msgs::PoseStamped::ConstPtr& ms marker_pose_ = transformed_pose; bool skip_plan = false; if (use_plane_snap_) { - if (!latest_grids_) { - ROS_WARN("no planes are available yet"); - } - else { - // do something magicalc - skip_plan = !projectMarkerToPlane(); - if (skip_plan) { - marker_pose_ = prev_pose; - } + // do something magicalc + skip_plan = !projectMarkerToPlane(); + if (skip_plan) { + marker_pose_ = prev_pose; } } @@ -898,19 +820,6 @@ void FootstepMarker::initializeInteractiveMarker() { FootstepMarker::~FootstepMarker() { } -void FootstepMarker::planeCB( - const jsk_recognition_msgs::SimpleOccupancyGridArray::ConstPtr& grid_msg) -{ - boost::mutex::scoped_lock lock(plane_mutex_); - ROS_INFO("new grid receievd"); - if (grid_msg->grids.size() > 0) { - latest_grids_ = grid_msg; - } - else { - ROS_INFO_STREAM(__FUNCTION__ << " the size of the grid is 0, ignore"); - } -} - int main(int argc, char** argv) { ros::init(argc, argv, "footstep_marker"); FootstepMarker marker;