void InputOutputLinControl::updateMapRobotPose(tf::Transform relative_transform, tf::StampedTransform current_icp_robot_pose, tf::StampedTransform& next_icp_robot_pose)
{
tf::Transform transformation = relative_transform * current_icp_robot_pose;
next_icp_robot_pose.frame_id_ = current_icp_robot_pose.frame_id_;
next_icp_robot_pose.stamp_ = ros::Time::now();
next_icp_robot_pose.child_frame_id_ = current_icp_robot_pose.child_frame_id_;
next_icp_robot_pose.setBasis(transformation.getBasis());
next_icp_robot_pose.setOrigin(transformation.getOrigin());
next_icp_robot_pose.setRotation(transformation.getRotation());
}
