Merge pull request jsk-ros-pkg#635 from YoheiKakiuchi/fix_footstep_of…

…fset_final

[footstep_planner/footstep_marker] fix offset

.travis.rosinstall.hydro

@@ -0,0 +1,4 @@
+- git:
+    uri: https://github.com/jsk-ros-pkg/jsk_common_msgs.git
+    local-name: jsk-ros-pkg/jsk_common_msgs
+    version: 4.0.0

jsk_footstep_planner/include/jsk_footstep_planner/footstep_marker.h

@@ -54,7 +54,6 @@
 #include "jsk_footstep_planner/FootstepMarkerConfig.h"
 #include <dynamic_reconfigure/server.h>
 #include <jsk_rviz_plugins/OverlayText.h>
-#include <tf/transform_listener.h>
 
 namespace jsk_footstep_planner
 {
@@ -115,7 +114,8 @@ namespace jsk_footstep_planner
     virtual PosePair::Ptr getLatestCurrentFootstepPoses();
     virtual PosePair::Ptr getCurrentFootstepPoses(const ros::Time& stamp);
     virtual PosePair::Ptr getDefaultFootstepPair();
-    virtual visualization_msgs::Marker makeFootstepMarker(FootstepTrans pose);
+    virtual visualization_msgs::Marker makeFootstepMarker(FootstepTrans pose,
+                                                          unsigned char leg);
     virtual void processFeedbackCB(
       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
     virtual void processMenuFeedbackCB(
@@ -244,9 +244,6 @@ namespace jsk_footstep_planner
 
     bool have_last_step_;
     jsk_footstep_msgs::Footstep last_steps_[2];
-
-    tf::TransformListener listener_;
-
   private:
 
   };

jsk_footstep_planner/include/jsk_footstep_planner/footstep_planner.h

@@ -137,6 +137,8 @@ namespace jsk_footstep_planner
     std::vector<Eigen::Affine3f> successors_;
     Eigen::Vector3f collision_bbox_size_;
     Eigen::Affine3f collision_bbox_offset_;
+    Eigen::Vector3f inv_lleg_footstep_offset_;
+    Eigen::Vector3f inv_rleg_footstep_offset_;
     std_msgs::Header latest_header_;
     // Common Parameters
     FootstepParameters parameters_;

jsk_footstep_planner/include/jsk_footstep_planner/footstep_state.h

@@ -120,6 +120,7 @@ namespace jsk_footstep_planner
       return a[0] * b[1] - a[1] * b[0];
     }
     virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg();
+    virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg(const Eigen::Vector3f& ioffset);
 #if 0
     virtual FootstepState::Ptr
     projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,

jsk_footstep_planner/launch/JAXON_RED_footstep_planner_perception.launch

@@ -7,12 +7,14 @@
   <arg name="use_go_pos_server" default="true"/>
   <arg name="use_floor_detection" default="false"/>
   <arg name="use_footstep_plane_detection" default="false"/>
+  <arg name="LAUNCH_TF_BUFFER_SERVER" default="false" />
 
   <arg if="$(arg USE_NORMAL)"     name="input_point_cloud"
        value="/footstep_normal_estimation/output_with_xyz" />
   <arg unless="$(arg USE_NORMAL)" name="input_point_cloud"
        value="/accumulated_heightmap_pointcloud_static/output" />
 
+  <!-- nodes for perception -->
   <node if="$(arg USE_NORMAL)" pkg="jsk_pcl_ros" type="normal_estimation_omp" name="footstep_normal_estimation">
     <remap from="~input" to="/accumulated_heightmap_pointcloud_static/output" />
     <rosparam subst_value="true">
@@ -21,8 +23,9 @@
       number_of_threads: 1
     </rosparam>
   </node>
-
   <include file="$(find jsk_robot_startup)/launch/slam_octomap.launch"/>
+
+  <!-- footstep planner -->
   <node pkg="jsk_footstep_planner" type="footstep_planner_node" name="footstep_planner" output="screen">
     <remap from="~pointcloud_model" to="$(arg input_point_cloud)" />
     <remap from="~obstacle_model" to="octomap_point_cloud_centers" />
@@ -41,9 +44,12 @@
     </rosparam>
     <!-- size of foot -->
     <rosparam>
-      ## add padding
-      footstep_size_x: 0.26
-      footstep_size_y: 0.145
+      ## offset from end-coords -> center of cube
+      lleg_footstep_offset: [0.015502,  0.010078, 0.0]
+      rleg_footstep_offset: [0.015502, -0.010078, 0.0]
+      ## without padding
+      footstep_size_x: 0.237418
+      footstep_size_y: 0.134017
     </rosparam>
     <!-- parameters for plane estimation -->
     <rosparam subst_value="true">
@@ -81,13 +87,16 @@
       support_check_x_sampling: 3
       support_check_y_sampling: 3
       support_check_vertex_neighbor_threshold: 0.02
-      support_padding_x: 0.0
-      support_padding_y: 0.0
+      support_padding_x: 0.0 ##
+      support_padding_y: 0.0 ##
     </rosparam>
     <!-- successors -->
     <rosparam>
-      ## default_rfoot_to_lfoot_offset: [x, y, theta]
-      default_rfoot_to_lfoot_offset: [0, -0.220156, 0]
+      ## default_lfoot_to_rfoot_offset: [x, y, theta]
+      ## default_lfoot_to_rfoot_offset: [0, -0.220156, 0] ## within offset end-coords to center of cube
+      default_lfoot_to_rfoot_offset: [0, -0.200, 0] ## just offset between end-coords of legs
+      ## successors is a list of transformations from left foot to right foot
+      ## if the parameter named 'default_lfoot_to_rfoot_offset' is set, it would be added to successors.
       successors:
 # stepping
       - x: 0
@@ -168,6 +177,8 @@
         theta: -0.09
     </rosparam>
   </node>
+
+  <!-- footstep marker -->
   <group if="$(arg use_footstep_marker)">
     <node pkg="jsk_footstep_planner" type="footstep_marker" name="footstep_marker" output="screen">
       <remap from="/footstep_controller" to="jaxon_footstep_controller"/>
@@ -213,5 +224,7 @@
   <node pkg="rviz" type="rviz" name="rviz" 
         args="-d $(find jsk_footstep_planner)/config/jaxon_footstep_planner_perception.rviz"
         if="$(arg rviz)"/>
-
+
+  <node pkg="tf2_ros" type="buffer_server" name="tf2_buffer_server"
+        if="$(arg LAUNCH_TF_BUFFER_SERVER)" />
 </launch>

jsk_footstep_planner/src/footstep_marker.cpp

@@ -210,15 +210,25 @@ namespace jsk_footstep_planner
     ros::Duration(1.0).sleep();
   }
 
-  visualization_msgs::Marker FootstepMarker::makeFootstepMarker(FootstepTrans pose)
+  visualization_msgs::Marker FootstepMarker::makeFootstepMarker(FootstepTrans pose, unsigned char leg)
   {
+    FootstepTrans footpose = pose;
+    if (leg == jsk_footstep_msgs::Footstep::LLEG) {
+      FootstepTranslation ltrans(lleg_footstep_offset_[0], lleg_footstep_offset_[1], lleg_footstep_offset_[2]);
+      footpose = pose * ltrans;
+    } else if (leg == jsk_footstep_msgs::Footstep::RLEG) {
+      FootstepTranslation rtrans(rleg_footstep_offset_[0], rleg_footstep_offset_[1], rleg_footstep_offset_[2]);
+      footpose = pose * rtrans;
+    } else {
+      ROS_ERROR ("makeFootstepMarker not implemented leg (%d)", leg);
+    }
     visualization_msgs::Marker marker;
     if (is_cube_mode_) {
       marker.type = visualization_msgs::Marker::CUBE;
       marker.scale.x = foot_size_x_;
       marker.scale.y = foot_size_y_;
       marker.scale.z = foot_size_z_;
-      tf::poseEigenToMsg(pose, marker.pose);
+      tf::poseEigenToMsg(footpose, marker.pose);
     }
     else {
       marker.type = visualization_msgs::Marker::LINE_STRIP;
@@ -227,10 +237,10 @@ namespace jsk_footstep_planner
       FootstepVec local_b(-foot_size_x_ / 2.0,  foot_size_y_ / 2.0, 0.0);
       FootstepVec local_c(-foot_size_x_ / 2.0, -foot_size_y_ / 2.0, 0.0);
       FootstepVec local_d( foot_size_x_ / 2.0, -foot_size_y_ / 2.0, 0.0);
-      FootstepVec a = pose * local_a;
-      FootstepVec b = pose * local_b;
-      FootstepVec c = pose * local_c;
-      FootstepVec d = pose * local_d;
+      FootstepVec a = footpose * local_a;
+      FootstepVec b = footpose * local_b;
+      FootstepVec c = footpose * local_c;
+      FootstepVec d = footpose * local_d;
       geometry_msgs::Point ros_a, ros_b, ros_c, ros_d;
       ros_a.x = a[0]; ros_a.y = a[1]; ros_a.z = a[2];
       ros_b.x = b[0]; ros_b.y = b[1]; ros_b.z = b[2];
@@ -252,9 +262,11 @@ namespace jsk_footstep_planner
   {
     FootstepTrans midcoords = leg_poses->midcoords();
     tf::poseEigenToMsg(midcoords, int_marker.pose);
-    visualization_msgs::Marker left_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(lleg_end_coords_));
+    visualization_msgs::Marker left_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(lleg_end_coords_),
+                                                                    jsk_footstep_msgs::Footstep::LLEG);
     left_box_marker.color.g = 1.0;
-    visualization_msgs::Marker right_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(rleg_end_coords_));
+    visualization_msgs::Marker right_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(rleg_end_coords_),
+                                                                     jsk_footstep_msgs::Footstep::RLEG);
     right_box_marker.color.r = 1.0;
     visualization_msgs::InteractiveMarkerControl left_box_control;
     left_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
@@ -278,9 +290,9 @@ namespace jsk_footstep_planner
     tf::poseEigenToMsg(pose, int_goal_marker.pose);
     current_lleg_offset_ = pose.inverse() * lleg_goal_pose_;
     current_rleg_offset_ = pose.inverse() * rleg_goal_pose_;
-    visualization_msgs::Marker left_box_marker = makeFootstepMarker(current_lleg_offset_);
+    visualization_msgs::Marker left_box_marker = makeFootstepMarker(current_lleg_offset_, jsk_footstep_msgs::Footstep::LLEG);
     left_box_marker.color.g = 1.0;
-    visualization_msgs::Marker right_box_marker = makeFootstepMarker(current_rleg_offset_);
+    visualization_msgs::Marker right_box_marker = makeFootstepMarker(current_rleg_offset_, jsk_footstep_msgs::Footstep::RLEG);
     right_box_marker.color.r = 1.0;
     visualization_msgs::InteractiveMarkerControl left_box_control;
     left_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
@@ -439,16 +451,6 @@ namespace jsk_footstep_planner
             last_steps_[0] = plan_result_.footsteps[size-2]; // left
             last_steps_[1] = plan_result_.footsteps[size-1]; // right
           }
-          Eigen::Affine3d lfoot;
-          Eigen::Translation3d ltrans(lleg_footstep_offset_[0], lleg_footstep_offset_[1], lleg_footstep_offset_[2]);
-          Eigen::Affine3d rfoot;
-          Eigen::Translation3d rtrans(rleg_footstep_offset_[0], rleg_footstep_offset_[1], rleg_footstep_offset_[2]);
-          tf::poseMsgToEigen(last_steps_[0].pose, lfoot);
-          tf::poseMsgToEigen(last_steps_[1].pose, rfoot);
-          lfoot *= ltrans;
-          rfoot *= rtrans;
-          tf::poseEigenToMsg(lfoot, last_steps_[0].pose);
-          tf::poseEigenToMsg(rfoot, last_steps_[1].pose);
         }
         have_last_step_ = true;
         if (goal.strategy == jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET) {
@@ -650,23 +652,6 @@ namespace jsk_footstep_planner
     pub_plan_result_.publish(result->result);
     planning_state_ = FINISHED;
     plan_result_ = result->result;
-    // subtracting offset
-    for (jsk_footstep_msgs::FootstepArray::_footsteps_type::iterator it = plan_result_.footsteps.begin();
-         it != plan_result_.footsteps.end(); it++) {
-      if( (*it).leg == jsk_footstep_msgs::Footstep::LEFT) {
-        Eigen::Translation3d trans(-lleg_footstep_offset_[0], -lleg_footstep_offset_[1], -lleg_footstep_offset_[2]);
-        Eigen::Affine3d new_pose;
-        tf::poseMsgToEigen((*it).pose, new_pose);
-        new_pose *= trans;
-        tf::poseEigenToMsg(new_pose, (*it).pose);
-      } else { // Right
-        Eigen::Translation3d trans(-rleg_footstep_offset_[0], -rleg_footstep_offset_[1], -rleg_footstep_offset_[2]);
-        Eigen::Affine3d new_pose;
-        tf::poseMsgToEigen((*it).pose, new_pose);
-        new_pose *= trans;
-        tf::poseEigenToMsg(new_pose, (*it).pose);
-      }
-    }
   }
 
   void FootstepMarker::planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
@@ -678,15 +663,11 @@ namespace jsk_footstep_planner
   }
 
   FootstepTrans FootstepMarker::getDefaultLeftLegOffset() {
-    return FootstepTrans(FootstepTranslation(lleg_footstep_offset_[0],
-                                             default_footstep_margin_ / 2.0 + lleg_footstep_offset_[1],
-                                             lleg_footstep_offset_[2]));
+    return FootstepTrans(FootstepTranslation(0, default_footstep_margin_ / 2.0, 0));
   }
 
   FootstepTrans FootstepMarker::getDefaultRightLegOffset() {
-    return FootstepTrans(FootstepTranslation(rleg_footstep_offset_[0],
-                                             - default_footstep_margin_ / 2.0 + rleg_footstep_offset_[1],
-                                             rleg_footstep_offset_[2]));
+    return FootstepTrans(FootstepTranslation(0, - default_footstep_margin_ / 2.0, 0));
   }
 
   void FootstepMarker::processFeedbackCB(
@@ -890,8 +871,8 @@ namespace jsk_footstep_planner
 
   PosePair::Ptr FootstepMarker::getDefaultFootstepPair()
   {
-    FootstepTrans lleg_default_pose(FootstepTranslation(0, 0.1, 0) * FootstepTranslation(lleg_footstep_offset_));
-    FootstepTrans rleg_default_pose(FootstepTranslation(0, -0.1, 0) * FootstepTranslation(rleg_footstep_offset_));
+    FootstepTrans lleg_default_pose(FootstepTranslation(0, 0.1, 0));
+    FootstepTrans rleg_default_pose(FootstepTranslation(0, -0.1, 0));
     return PosePair::Ptr(new PosePair(lleg_default_pose, lleg_end_coords_,
                                       rleg_default_pose, rleg_end_coords_));
   }
@@ -922,8 +903,8 @@ namespace jsk_footstep_planner
     FootstepTrans lleg_transform_eigen, rleg_transform_eigen;
     tf::transformMsgToEigen(lleg_transform.transform, lleg_transform_eigen);
     tf::transformMsgToEigen(rleg_transform.transform, rleg_transform_eigen);
-    return PosePair::Ptr(new PosePair(lleg_transform_eigen * FootstepTranslation(lleg_footstep_offset_), lleg_end_coords_,
-                                      rleg_transform_eigen * FootstepTranslation(rleg_footstep_offset_), rleg_end_coords_));
+    return PosePair::Ptr(new PosePair(lleg_transform_eigen, lleg_end_coords_,
+                                      rleg_transform_eigen, rleg_end_coords_));
   }
 
   void FootstepMarker::configCallback(Config &config, uint32_t level)
@@ -943,12 +924,19 @@ namespace jsk_footstep_planner
       // apply target pose to goal marker
       // mutex should be limited in this range because planIfPossible also lock planner_mutex_
       boost::mutex::scoped_lock lock(planner_mutex_);
-      FootstepTrans eigen_command_pose;
-
       try {
-        listener_.waitForTransform(odom_frame_id_, msg->header.frame_id, msg->header.stamp, ros::Duration(2.0));
-        listener_.transformPose(odom_frame_id_, *msg, tmp_pose_stamped);
-      } catch(tf::TransformException ex) {
+        geometry_msgs::TransformStamped offset = tf_client_->lookupTransform(odom_frame_id_, msg->header.frame_id,
+                                                                             msg->header.stamp, ros::Duration(2.0));
+        FootstepTrans msg_eigen;
+        FootstepTrans offset_eigen;
+        tf::poseMsgToEigen(msg->pose, msg_eigen);
+        tf::transformMsgToEigen(offset.transform, offset_eigen);
+        FootstepTrans pose = msg_eigen * offset_eigen;
+
+        tf::poseEigenToMsg(pose, tmp_pose_stamped.pose);
+        tmp_pose_stamped.header.stamp = msg->header.stamp;
+        tmp_pose_stamped.header.frame_id = odom_frame_id_;
+      } catch(tf2::TransformException ex) {
         ROS_ERROR("posestamped command transformation failed %s",ex.what());
         return;
       }