[jsk_footstep_controller] add simple_footcoords

jsk_footstep_controller/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
   jsk_pcl_ros
   tf_conversions
   std_msgs
+  std_srvs
   message_generation genmsg message_filters sensor_msgs geometry_msgs tf jsk_topic_tools
   eigen_conversions
   kdl_parser
@@ -43,7 +44,10 @@ add_executable(footcoords src/footcoords.cpp)
 target_link_libraries(footcoords ${catkin_LIBRARIES})
 add_dependencies(footcoords ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_gencpp)
 
-install(TARGETS footcoords
+add_executable(simple_footcoords src/simple_footcoords.cpp)
+target_link_libraries(simple_footcoords ${catkin_LIBRARIES})
+
+install(TARGETS footcoords simple_footcoords
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

jsk_footstep_controller/include/jsk_footstep_controller/simple_footcoords.h

@@ -0,0 +1,50 @@
+#ifndef JSK_FOOTSTEP_CONTROLLER_SIMPLE_FOOTCOORDS_H_
+#define JSK_FOOTSTEP_CONTROLLER_SIMPLE_FOOTCOORDS_H_
+
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+#include <std_srvs/SetBool.h>
+#include <Eigen/Geometry>
+
+namespace jsk_footstep_controller
+{
+  /*
+    SimpleFootcoords
+      Published Topics:
+        odom_base_footprint(nsv_msgs/Odometry): "odom"->"base_footprint"
+      Subscribed Topics:
+        odom
+      Provided TF Transforms:
+        odom.header.frame_id (e.g. "odom")->odom.child_frame_id (e.g. "BODY"): same as the odom topic
+        "odom"->"base_footprint": roll and pitch are zero respect to "odom". z = min(`rfoot_frame_id`_z, `lfoot_frame_id`_z). x, y and yaw are the barycenter of `rfoot_frame_id` and `lfoot_frame_id`
+      Services:
+        ~enable(std_srvs::setBool): start / stop
+      Required TF:
+        "odom"->odom.header.frame_id
+   */
+  class SimpleFootcoords
+  {
+  public:
+    SimpleFootcoords();
+
+    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
+    bool enableCallback(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response);
+    bool waitForEndEffectorTransformation(const ros::Time& stamp);
+  protected:
+    bool isActive_;
+    Eigen::Affine3d odom_pose_;
+
+    boost::shared_ptr<tf::TransformListener> tfListener_;
+    tf::TransformBroadcaster tfBroadcaster_;
+    ros::Subscriber odomSub_;
+    ros::Publisher odomBaseFootPrintPub_;
+    ros::ServiceServer enableService_;
+
+    std::string lfoot_frame_id_;
+    std::string rfoot_frame_id_;
+  private:
+  };
+}
+#endif
+

jsk_footstep_controller/launch/hrp2jsk_footcoords.launch

@@ -1,12 +1,20 @@
 <launch>
   <arg name="use_footcoords" default="true"/>
+  <arg name="use_simple_footcoords" default="false"/>
   <node pkg="jsk_footstep_controller"
         type="footcoords"
         name="footcoords"
         if="$(arg use_footcoords)"
         >
   </node>
 
+  <node pkg="jsk_footstep_controller"
+        type="simple_footcoords"
+        name="simple_footcoords"
+        if="$(arg use_simple_footcoords)"
+        >
+  </node>
+
   <node pkg="jsk_footstep_controller"
         type="stabilizer_watcher.py"
         name="stabilizer_watcher" />

jsk_footstep_controller/package.xml

@@ -43,6 +43,8 @@
   <run_depend>tf_conversions</run_depend>
   <build_depend>std_msgs</build_depend>
   <run_depend>std_msgs</run_depend>
+  <build_depend>std_srvs</build_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>kdl_parser</run_depend>
   <run_depend>urdf</run_depend>
   <run_depend>kdl_conversions</run_depend>

jsk_footstep_controller/src/simple_footcoords.cpp

@@ -0,0 +1,134 @@
+#include "jsk_footstep_controller/simple_footcoords.h"
+#include <tf_conversions/tf_eigen.h>
+#include <eigen_conversions/eigen_msg.h>
+
+namespace jsk_footstep_controller{
+
+  SimpleFootcoords::SimpleFootcoords():
+    isActive_(true)
+  {
+    ros::NodeHandle nh, pnh("~");
+    tfListener_.reset(new tf::TransformListener());
+
+    pnh.param("lfoot_frame_id", lfoot_frame_id_,
+              std::string("lleg_end_coords"));
+    pnh.param("rfoot_frame_id", rfoot_frame_id_,
+              std::string("rleg_end_coords"));
+
+    odomSub_ = nh.subscribe("odom", 3, &SimpleFootcoords::odomCallback, this); // keep only latest ones to avoid latency because waitForTransform in callback function takes much time.
+    odomBaseFootPrintPub_ = nh.advertise<nav_msgs::Odometry>("odom_base_footprint", 1);
+    enableService_ = pnh.advertiseService("enable", &SimpleFootcoords::enableCallback, this);
+  }
+
+  void SimpleFootcoords::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
+    if (!isActive_) return;
+
+    std::vector<geometry_msgs::TransformStamped> tf_transforms;
+
+    // odom
+    Eigen::Affine3d odom_pose;
+    tf::poseMsgToEigen(msg->pose.pose, odom_pose);
+    geometry_msgs::TransformStamped odom_coords;
+    odom_coords.header = msg->header;
+    odom_coords.child_frame_id = msg->child_frame_id;
+    tf::transformEigenToMsg(odom_pose, odom_coords.transform);
+    tf_transforms.push_back(odom_coords);
+    tfBroadcaster_.sendTransform(tf_transforms); // odom tf is used below so odom tf need to be published here
+    tf_transforms.clear();
+
+    // base_footprint
+    if(!waitForEndEffectorTransformation(msg->header.stamp)) {
+      ROS_ERROR("[SimpleFootcoords::computeMidCoords] Failed to lookup endeffector transformation");
+    } else {
+      tf::StampedTransform lfoot_transform;
+      tfListener_->lookupTransform("odom", lfoot_frame_id_, msg->header.stamp, lfoot_transform);
+      Eigen::Affine3d lfoot_pose;
+      tf::transformTFToEigen(lfoot_transform,lfoot_pose);
+
+      tf::StampedTransform rfoot_transform;
+      tfListener_->lookupTransform("odom", rfoot_frame_id_, msg->header.stamp, rfoot_transform);
+      Eigen::Affine3d rfoot_pose;
+      tf::transformTFToEigen(rfoot_transform,rfoot_pose);
+
+      Eigen::Affine3d base_footprint_pose;
+      base_footprint_pose.translation()(0) = (rfoot_pose.translation()(0) + lfoot_pose.translation()(0)) / 2.0;
+      base_footprint_pose.translation()(1) = (rfoot_pose.translation()(1) + lfoot_pose.translation()(1)) / 2.0;
+      base_footprint_pose.translation()(2) = std::min(rfoot_pose.translation()(2), lfoot_pose.translation()(2));
+
+      Eigen::Quaterniond lfoot_quat(lfoot_pose.linear());
+      Eigen::Quaterniond rfoot_quat(rfoot_pose.linear());
+      Eigen::Quaterniond mid_quat = lfoot_quat.slerp(0.5,rfoot_quat);
+      Eigen::Matrix3d mid_rot = mid_quat.toRotationMatrix();
+      // roll and pitch should be zero respect to odom
+      Eigen::Vector3d axis = (mid_rot*Eigen::Vector3d::UnitZ()).cross(Eigen::Vector3d::UnitZ());
+      if(axis.norm()!=0){
+        double sin = axis.norm();
+        double cos = (mid_rot*Eigen::Vector3d::UnitZ()).dot(Eigen::Vector3d::UnitZ());
+        double angle = std::atan2(sin,cos);
+        mid_rot = (Eigen::AngleAxisd(angle,axis.normalized()).toRotationMatrix() * mid_rot).eval();
+      }
+      base_footprint_pose.linear() = mid_rot;
+
+      geometry_msgs::TransformStamped base_footprint_coords;
+      base_footprint_coords.header = msg->header;
+      base_footprint_coords.header.frame_id = "odom";
+      base_footprint_coords.child_frame_id = "base_footprint";
+      tf::transformEigenToMsg(base_footprint_pose, base_footprint_coords.transform);
+      tf_transforms.push_back(base_footprint_coords);
+
+      nav_msgs::Odometry base_footprint_msg;
+      base_footprint_msg.header = msg->header;
+      base_footprint_msg.header.frame_id = "odom";
+      base_footprint_msg.child_frame_id = "base_footprint";
+      tf::poseEigenToMsg(base_footprint_pose,base_footprint_msg.pose.pose);
+      odomBaseFootPrintPub_.publish(base_footprint_msg);
+    }
+
+    tfBroadcaster_.sendTransform(tf_transforms);
+  }
+
+  bool SimpleFootcoords::enableCallback(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) {
+    if(isActive_ == request.data){
+      ROS_INFO("[SimpleFootcoords::enableCallback] Already %s",(isActive_ ? "Enabled" : "Disabled"));
+      response.success = true;
+      response.message = "";
+      return true;
+    } else {
+      isActive_ = request.data;
+
+      ROS_INFO("[SimpleFootcoords::enableCallback] %s",(isActive_ ? "Enabled" : "Disabled"));
+      response.success = true;
+      response.message = "";
+      return true;
+    }
+  }
+
+  bool SimpleFootcoords::waitForEndEffectorTransformation(const ros::Time& stamp)
+  {
+    // root -> lfoot
+    if (!tfListener_->waitForTransform("odom", lfoot_frame_id_, stamp, ros::Duration(1.0))) {
+      ROS_ERROR("[SimpleFootcoords::waitForEndEffectorTrasnformation] failed to lookup transform between %s and %s",
+                "odom",
+                lfoot_frame_id_.c_str());
+      return false;
+    }
+    // root -> rfoot
+    else if (!tfListener_->waitForTransform("odom", rfoot_frame_id_, stamp, ros::Duration(1.0))) {
+      ROS_ERROR("[SimpleFootcoords::waitForEndEffectorTrasnformation]failed to lookup transform between %s and %s",
+                "odom",
+                rfoot_frame_id_.c_str());
+      return false;
+    }
+    return true;
+  }
+
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "simple_footcoords");
+  jsk_footstep_controller::SimpleFootcoords c;
+  ros::AsyncSpinner spinner(10); // Use many threads to increase frequency because waitForTransform in callback function takes much time.
+  spinner.start();
+  ros::waitForShutdown();
+}