18 #ifndef NAO_FOOTPRINT_HPP 19 #define NAO_FOOTPRINT_HPP 25 #include <geometry_msgs/Transform.h> 31 #include "../helpers/transform_helpers.hpp" 42 bool canTransform = tf2_buffer->canTransform(
"odom",
"l_sole", time,
ros::Duration(0.1) );
45 ROS_ERROR_STREAM(
"Do not calculate NAO Footprint, no transform possible " << time);
49 geometry_msgs::TransformStamped tf_odom_to_base, tf_odom_to_left_foot, tf_odom_to_right_foot;
52 tf_odom_to_left_foot = tf2_buffer->lookupTransform(
"odom",
"l_sole", time );
53 tf_odom_to_right_foot = tf2_buffer->lookupTransform(
"odom",
"r_sole", time );
54 tf_odom_to_base = tf2_buffer->lookupTransform(
"odom",
"base_link", time );
56 ROS_ERROR(
"NAO Footprint error %s",ex.what());
61 tf2::Vector3 new_origin(
62 float(tf_odom_to_right_foot.transform.translation.x + tf_odom_to_left_foot.transform.translation.x)/2.0,
63 float(tf_odom_to_right_foot.transform.translation.y + tf_odom_to_left_foot.transform.translation.y)/2.0,
64 std::min(tf_odom_to_left_foot.transform.translation.z, tf_odom_to_right_foot.transform.translation.z)
75 tf_odom_to_base.transform.rotation.y,
76 tf_odom_to_base.transform.rotation.z,
77 tf_odom_to_base.transform.rotation.w
79 tf2::Vector3 r( tf_odom_to_base.transform.translation.x,
80 tf_odom_to_base.transform.translation.y,
81 tf_odom_to_base.transform.translation.z
89 geometry_msgs::TransformStamped message;
91 message.header.stamp = time;
92 message.header.frame_id =
"base_link";
93 message.child_frame_id =
"base_footprint";
95 message.transform.rotation.x = tf_base_to_footprint.
getRotation().x();
96 message.transform.rotation.y = tf_base_to_footprint.
getRotation().y();
97 message.transform.rotation.z = tf_base_to_footprint.
getRotation().z();
98 message.transform.rotation.w = tf_base_to_footprint.
getRotation().w();
99 message.transform.translation.x = tf_base_to_footprint.
getOrigin().x();
100 message.transform.translation.y = tf_base_to_footprint.
getOrigin().y();
101 message.transform.translation.z = tf_base_to_footprint.
getOrigin().z();
106 tf2_buffer->setTransform( message,
"naoqiconverter",
false );
107 tf_transforms.push_back( message );
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void addBaseFootprint(boost::shared_ptr< tf2_ros::Buffer > tf2_buffer, std::vector< geometry_msgs::TransformStamped > &tf_transforms, const ros::Time &time)
#define ROS_ERROR_STREAM(args)