Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[footcoords] publish base_footprint #762

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[footcoords] publish base_footprint #762

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fe14fbe
[footcoords] publish base_footprint
Naoki-Hiraoka May 9, 2021
6b11b6e
Merge branch 'master' into base-footprint
k-okada May 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions jsk_footstep_controller/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ by `~lleg_end_coords` and `~rleg_end_coords`.
* `/odom_on_ground`: The coordinate computing by projecting `/odom` on the plane which is defined by `/ground` frame. The transformation is updated only if the robot on the floow by checking
both of `lfsensor` and `rfsensor` provides enough force (the threshold is `~force_threshold`).
* `/odom_init`: The coordinate of `/odom` when the robot lands on the ground most recent.
* `/base_footprint`: Roll and Pitch are zero respect to `~parent_frame_id`. 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`.

### Subscribing Topics
* `lfsensosor` (`geometry_msgs/WrenchStamped`)
Expand Down
1 change: 1 addition & 0 deletions jsk_footstep_controller/include/jsk_footstep_controller/footcoords.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,7 @@ namespace jsk_footstep_controller
// ros variables
boost::mutex mutex_;
Eigen::Affine3d odom_pose_;
Eigen::Affine3d root_to_rfoot_pose_, root_to_lfoot_pose_;
Eigen::Affine3d odom_init_pose_;
ros::Timer periodic_update_timer_;
ros::Subscriber odom_init_trigger_sub_;
Expand Down
24 changes: 23 additions & 1 deletion jsk_footstep_controller/src/footcoords.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,9 +55,12 @@ namespace jsk_footstep_controller
tf_listener_.reset(new tf::TransformListener());
odom_status_ = UNINITIALIZED;
odom_pose_ = Eigen::Affine3d::Identity();
root_to_rfoot_pose_ = Eigen::Affine3d::Identity();
root_to_lfoot_pose_ = Eigen::Affine3d::Identity();
ground_transform_.setRotation(tf::Quaternion(0, 0, 0, 1));
root_link_pose_.setIdentity();
midcoords_.setRotation(tf::Quaternion(0, 0, 0, 1));
midcoords_.setRotation(tf::Quaternion(0, 0, 0, 1));
estimated_odom_pose_.setRotation(tf::Quaternion(0, 0, 0, 1));
diagnostic_updater_->setHardwareID("none");
diagnostic_updater_->add("Support Leg Status", this,
Expand Down Expand Up @@ -815,9 +818,11 @@ namespace jsk_footstep_controller
tf::StampedTransform lfoot_transform;
tf_listener_->lookupTransform(
root_frame_id_, lfoot_frame_id_, stamp, lfoot_transform);
tf::transformTFToEigen(lfoot_transform, root_to_lfoot_pose_);
tf::StampedTransform rfoot_transform;
tf_listener_->lookupTransform(
root_frame_id_, rfoot_frame_id_, stamp, rfoot_transform);
tf::transformTFToEigen(rfoot_transform, root_to_rfoot_pose_);
tf::Quaternion lfoot_rot = lfoot_transform.getRotation();
tf::Quaternion rfoot_rot = rfoot_transform.getRotation();
tf::Quaternion mid_rot = lfoot_rot.slerp(rfoot_rot, 0.5);
Expand Down Expand Up @@ -955,7 +960,7 @@ namespace jsk_footstep_controller
{
// publish midcoords_ and ground_cooords_
geometry_msgs::TransformStamped ros_midcoords,
ros_ground_coords, ros_odom_to_body_coords,
ros_ground_coords, ros_odom_to_body_coords, ros_odom_to_base_footprint_coords,
ros_body_on_odom_coords, ros_odom_init_coords;
// ros_midcoords: ROOT -> ground
// ros_ground_coords: odom -> odom_on_ground = identity
Expand All @@ -973,6 +978,9 @@ namespace jsk_footstep_controller
ros_odom_to_body_coords.header.stamp = stamp;
ros_odom_to_body_coords.header.frame_id = parent_frame_id_;
ros_odom_to_body_coords.child_frame_id = root_frame_id_;
ros_odom_to_base_footprint_coords.header.stamp = stamp;
ros_odom_to_base_footprint_coords.header.frame_id = parent_frame_id_;
ros_odom_to_base_footprint_coords.child_frame_id = "base_footprint";
ros_body_on_odom_coords.header.stamp = stamp;
ros_body_on_odom_coords.header.frame_id = root_frame_id_;
ros_body_on_odom_coords.child_frame_id = body_on_odom_frame_;
Expand Down Expand Up @@ -1002,6 +1010,19 @@ namespace jsk_footstep_controller
tf::transformEigenToMsg(identity, ros_ground_coords.transform);
tf::transformEigenToMsg(body_on_odom_pose.inverse(), ros_body_on_odom_coords.transform);
tf::transformEigenToMsg(odom_pose_, ros_odom_to_body_coords.transform);
{
Eigen::Affine3d odom_to_base_footprint_pose;
Eigen::Affine3d odom_to_rfoot_pose = odom_pose_ * root_to_rfoot_pose_;
Eigen::Affine3d odom_to_lfoot_pose = odom_pose_ * root_to_lfoot_pose_;
odom_to_base_footprint_pose.translation()(0) = (odom_to_rfoot_pose.translation()(0) + odom_to_lfoot_pose.translation()(0)) / 2.0;
odom_to_base_footprint_pose.translation()(1) = (odom_to_rfoot_pose.translation()(1) + odom_to_lfoot_pose.translation()(1)) / 2.0;
odom_to_base_footprint_pose.translation()(2) = std::min(odom_to_rfoot_pose.translation()(2), odom_to_lfoot_pose.translation()(2));
odom_to_base_footprint_pose.linear() = Eigen::Quaterniond(odom_to_rfoot_pose.linear()).slerp(0.5, Eigen::Quaterniond(odom_to_lfoot_pose.linear())).toRotationMatrix();
Eigen::Quaterniond rot;
rot.setFromTwoVectors(odom_to_base_footprint_pose.linear() * Eigen::Vector3d::UnitZ(), Eigen::Vector3d::UnitZ());
odom_to_base_footprint_pose.linear() = (odom_to_base_footprint_pose.linear() * rot).eval();
tf::transformEigenToMsg(odom_to_base_footprint_pose, ros_odom_to_base_footprint_coords.transform);
}
if (invert_odom_init_) {
tf::transformEigenToMsg(odom_init_pose.inverse(), ros_odom_init_coords.transform);
} else {
Expand All @@ -1012,6 +1033,7 @@ namespace jsk_footstep_controller
tf_transforms.push_back(ros_ground_coords);
if (publish_odom_tf_) {
tf_transforms.push_back(ros_odom_to_body_coords);
tf_transforms.push_back(ros_odom_to_base_footprint_coords);
}
tf_transforms.push_back(ros_body_on_odom_coords);
tf_transforms.push_back(ros_odom_init_coords);
Expand Down