Skip to content

Commit

Permalink
Merge pull request #437 from garaemon/dynamic-reconfigrue-api
Browse files Browse the repository at this point in the history 
[jsk_interactive_marker/footstep_marker] Add dynamic reconfirue API t…
  • Loading branch information
@garaemon
garaemon committed Aug 4, 2015
2 parents 1f4420b + 3a8771f commit 84b1242
Show file tree
Hide file tree
Showing 4 changed files with 50 additions and 118 deletions.
1 change: 1 addition & 0 deletions jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
20 changes: 20 additions & 0 deletions jsk_interactive_markers/jsk_interactive_marker/cfg/FootstepMarker.cfg
View file
Original file line number Diff line number Diff line change
@@ -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"))
10 changes: 6 additions & 4 deletions ...teractive_markers/jsk_interactive_marker/include/jsk_interactive_marker/footstep_marker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@

#include <jsk_recognition_msgs/PolygonArray.h>
#include <jsk_recognition_msgs/ModelCoefficientsArray.h>
#include <jsk_interactive_marker/FootstepMarkerConfig.h>

#include <interactive_markers/menu_handler.h>
#include <jsk_interactive_marker/SetPose.h>
Expand All @@ -57,9 +58,11 @@
#include <std_msgs/UInt8.h>
#include <std_msgs/Empty.h>
#include <jsk_recognition_msgs/SimpleOccupancyGridArray.h>
#include <dynamic_reconfigure/server.h>

class FootstepMarker {
public:
typedef jsk_interactive_marker::FootstepMarkerConfig Config;
FootstepMarker();
virtual ~FootstepMarker();
void updateInitialFootstep();
Expand All @@ -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);
Expand All @@ -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<dynamic_reconfigure::Server<Config> > srv_;
// projection to the planes
bool projectMarkerToPlane();

Expand Down Expand Up @@ -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_;
Expand All @@ -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_;
Expand Down
137 changes: 23 additions & 114 deletions jsk_interactive_markers/jsk_interactive_marker/src/footstep_marker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <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("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<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_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);
Expand All @@ -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_);
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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<jsk_pcl_ros::GridPlane::Ptr> 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>(
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) {
Expand Down Expand Up @@ -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;
}
}

Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 84b1242

Please sign in to comment.