コード例 #1
0
 void poseKDLToTF(const KDL::Frame& k, tf::Pose& t)
 {
   t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
   t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
                          k.M.data[3], k.M.data[4], k.M.data[5],
                          k.M.data[6], k.M.data[7], k.M.data[8]));
 }
コード例 #2
0
 void poseTFToKDL(const tf::Pose& t, KDL::Frame& k)
 {
   for (unsigned int i = 0; i < 3; ++i)
     k.p[i] = t.getOrigin()[i];
   for (unsigned int i = 0; i < 9; ++i)
     k.M.data[i] = t.getBasis()[i/3][i%3];
 }
コード例 #3
0
// copied from channelcontroller
double CalibrateAction::getDistanceAtPose(const tf::Pose & pose, bool* in_bounds) const
{
    // determine current dist
    int pose_x, pose_y;
    costmap_.worldToMapNoBounds(pose.getOrigin().x(), pose.getOrigin().y(),
            pose_x, pose_y);
    //ROS_INFO("pose_x: %i, pose_y: %i", pose_x, pose_y);
    if(pose_x < 0 || pose_y < 0 ||
            pose_x >= (int)voronoi_.getSizeX() || pose_y >= (int)voronoi_.getSizeY()) {
        if(in_bounds == NULL) {
            // only warn if in_bounds isn't checked externally
            ROS_WARN_THROTTLE(1.0, "%s: Pose out of voronoi bounds (%.2f, %.2f) = (%d, %d)", __func__,
                    pose.getOrigin().x(), pose.getOrigin().y(), pose_x, pose_y);
        } else {
            *in_bounds = false;
        }
        return 0.0;
    }
    if(in_bounds)  {
        *in_bounds = true;
    }
    float dist = voronoi_.getDistance(pose_x, pose_y);
    dist *= costmap_.getResolution();
    return dist;
}
コード例 #4
0
	void toPose(const MathLib::Matrix4& mat4, tf::Pose& pose) {
		MathLib::Matrix3 m1 = mat4.GetOrientation();
		MathLib::Vector3 v1 = m1.GetRotationAxis();
		tf::Vector3 ax(v1(0), v1(1), v1(2));
		pose.setRotation(tf::Quaternion(ax, m1.GetRotationAngle()));
		v1.Set(mat4.GetTranslation());
		pose.setOrigin(tf::Vector3(v1(0),v1(1),v1(2)));
	}
コード例 #5
0
bool MarkerSearcherComputation::getNextSearchPose(tf::Pose& returnPose) {
    if(poseReturned) {
        return false;
    }
    returnPose.setOrigin(nextSearchPose.getOrigin());
    returnPose.setRotation(nextSearchPose.getRotation());
    poseReturned = true;
    return true;
}
コード例 #6
0
ファイル: RvizMarker.cpp プロジェクト: johnmichaloski/ROS
int CRvizMarker::Send(tf::Pose p, std::string frame) {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = frame;
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "nist_fanuc";
    marker.id = _id++;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD and DELETE
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
#if 0
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
#endif
    marker.pose.position.x = p.getOrigin().x();
    marker.pose.position.y = p.getOrigin().y();
    marker.pose.position.z = p.getOrigin().z();
    marker.pose.orientation.x = p.getRotation().x();
    marker.pose.orientation.y = p.getRotation().y();
    marker.pose.orientation.z = p.getRotation().z();
    marker.pose.orientation.w = p.getRotation().w();

    // Set the scale of the marker -- 1x1x1 here means 1m on a side!!!
    marker.scale.x = scalex;
    marker.scale.y = scaley;
    marker.scale.z = scalez;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;
    marker.color.a = a;

    marker.lifetime = ros::Duration(0.0);

    // Publish the marker
    marker_pub.publish(marker);
    ros::spinOnce();
    ros::spinOnce();

    return 0;
}
コード例 #7
0
    void toTfPose(const geometry_msgs::Pose& msg_pose, tf::Pose& tf_pose)
    {
        tf_pose.setOrigin(tf::Vector3(msg_pose.position.x,
                                      msg_pose.position.y,
                                      msg_pose.position.z));

        tf_pose.setRotation(tf::Quaternion(msg_pose.orientation.x,
                                           msg_pose.orientation.y,
                                           msg_pose.orientation.z,
                                           msg_pose.orientation.w));
    }
コード例 #8
0
	MathLib::Matrix4 toMatrix4(const tf::Pose& pose) {
		MathLib::Matrix4 mat;
		mat.Identity();
		tf::Matrix3x3 mat33(pose.getRotation());

		mat.SetTranslation(MathLib::Vector3(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()));
		mat.SetOrientation(MathLib::Matrix3(mat33[0][0], mat33[0][1], mat33[0][2],
				mat33[1][0], mat33[1][1], mat33[1][2],
				mat33[2][0], mat33[2][1], mat33[2][2]));
		return mat;
	}
コード例 #9
0
ファイル: sptam_node.cpp プロジェクト: Aerobota/sptam
// Convert from tf::Pose to CameraPose (position and orientation)
inline void TFPose2CameraPose(const tf::Pose& pose, CameraPose& cameraPose)
{
  // convert to position opencv vector
  tf::Vector3 position_tf = pose.getOrigin();
  cv::Point3d position = cv::Point3d(position_tf.getX(), position_tf.getY(), position_tf.getZ());

  // Convert to orientation opencv quaternion
  tf::Quaternion orientation_tf = pose.getRotation();
  cv::Vec4d orientation(orientation_tf.getW(), orientation_tf.getX(), orientation_tf.getY(), orientation_tf.getZ());

  cameraPose = CameraPose(position, orientation);
}
コード例 #10
0
    void toMsgPose(const tf::Pose& tf_pose, geometry_msgs::Pose& msg_pose)
    {
        msg_pose.position.x = tf_pose.getOrigin().getX();
        msg_pose.position.y = tf_pose.getOrigin().getY();
        msg_pose.position.z = tf_pose.getOrigin().getZ();

        tf::Quaternion orientation = tf_pose.getRotation();
        msg_pose.orientation.w = orientation.getW();
        msg_pose.orientation.x = orientation.getX();
        msg_pose.orientation.y = orientation.getY();
        msg_pose.orientation.z = orientation.getZ();
    }
コード例 #11
0
geometry_msgs::Pose tfTransformToGeometryPose(const tf::Pose& goal_pose)
{
    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.x = goal_pose.getRotation().getX();
    target_pose1.orientation.y = goal_pose.getRotation().getY();
    target_pose1.orientation.z = goal_pose.getRotation().getZ();
    target_pose1.orientation.w = goal_pose.getRotation().getW();
    target_pose1.position.x = goal_pose.getOrigin().getX(); // + std::sin(angle)*radius;
    target_pose1.position.y = goal_pose.getOrigin().getY(); // + std::cos(angle)*radius;
    target_pose1.position.z = goal_pose.getOrigin().getZ();
    return target_pose1;
}
コード例 #12
0
// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
	ROS_INFO_STREAM("Received Position Command");
	const geometry_msgs::PoseStamped* data = msg.get();
	des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
	des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));

	ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());

	if(bOrientCtrl) {
		tf::Quaternion q = des_ee_pose.getRotation();
		ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
	}
}
コード例 #13
0
geometry_msgs::Pose tfPoseToGeometryPose(const tf::Pose tPose)
{
    geometry_msgs::Pose gPose;
    gPose.position.x = tPose.getOrigin().x();
    gPose.position.y = tPose.getOrigin().y();
    gPose.position.z = tPose.getOrigin().z();
    gPose.orientation.x = tPose.getRotation().x();
    gPose.orientation.y = tPose.getRotation().y();
    gPose.orientation.z = tPose.getRotation().z();
    gPose.orientation.w = tPose.getRotation().w();

    return gPose;
}
コード例 #14
0
ファイル: motionmodel.cpp プロジェクト: iou16/3d_ogmapping
  void MotionModel::setMotion(const tf::Pose& pnew, const tf::Pose& pold)
    {
      tf::Pose delta_pose;
      tf::Transform odom_to_base(pold.inverse().getRotation());
      delta_pose.setOrigin(odom_to_base * (pnew.getOrigin() - pold.getOrigin()));
      delta_pose.setRotation(pnew.getRotation() * pold.getRotation().inverse());

      delta_x = delta_pose.getOrigin().x();
      delta_y = delta_pose.getOrigin().y();
      delta_yaw = atan2(sin(tf::getYaw(delta_pose.getRotation())), cos(tf::getYaw(delta_pose.getRotation())));

			dxy=sqrt(delta_x*delta_x+delta_y*delta_y);
    }
コード例 #15
0
static RVector<double> PoseValues(tf::Pose & pose) {
   RVector<double> values;
    values.push_back( pose.getOrigin().x());
    values.push_back( pose.getOrigin().y());
    values.push_back( pose.getOrigin().z());
    double roll, pitch, yaw;
    tf::Quaternion q = pose.getRotation();
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw );
    values.push_back( roll );
    values.push_back( pitch );
    values.push_back( yaw );
    return values;
}
コード例 #16
0
	// The famous sendPose!
	void sendPose(const tf::Pose& pose_) {
		geometry_msgs::PoseStamped msg;
		msg.pose.position.x = pose_.getOrigin().x();
		msg.pose.position.y = pose_.getOrigin().y();
		msg.pose.position.z = pose_.getOrigin().z();

		msg.pose.orientation.x = pose_.getRotation().x();
		msg.pose.orientation.y = pose_.getRotation().y();
		msg.pose.orientation.z = pose_.getRotation().z();
		msg.pose.orientation.w = pose_.getRotation().w();

		pub_.publish(msg);
	}
コード例 #17
0
pair<Nodes, PointVec>
nodesOnGrid (const unsigned g, const Roadmap& r,
             const tmap::TopologicalMap& tmap)
{
  pair<Nodes, PointVec> res;

  // Add all nodes defined wrt this grid
  BOOST_FOREACH (const unsigned n, r.gridNodes(g))
  {
    res.first.push_back(n);
    res.second.push_back(r.nodeInfo(n).position);
  }

  const double x_size = tmap.nodeInfo(g).x_size;
  const double y_size = tmap.nodeInfo(g).y_size;

  // Also look at neighboring grids in topological graph
  BOOST_FOREACH (const tmap::GraphEdge e,
                 out_edges(tmap.node(g), tmap)) 
  {
    const bool flip = (tmap[e].dest == g);
    const tf::Pose offset = gmu::toPose(tmap[e].offset);
    const tf::Transform tr = flip ? offset.inverse() : offset;
    const unsigned g2 = flip ? tmap[e].src : tmap[e].dest;
    // Now tr maps from the neighbor grid g2 to g

    BOOST_FOREACH (const unsigned n, r.gridNodes(g2))
    {
      ROS_ASSERT(g2==r.nodeInfo(n).grid);
      const gm::Point& pos = r.nodeInfo(n).position;
      const gm::Point local_pos = gmu::transformPoint(tr, pos);
      // Now local_pos is the position of the waypoint in frame g

      ROS_DEBUG_STREAM_NAMED ("nodes_on_grid", "Node " << n << " position on " << g2 << " is "
                              << gmu::toString(pos) << " and on " << g <<
                              " is " << gmu::toString(local_pos));

      if (fabs(local_pos.x) < x_size/2 && fabs(local_pos.y) < y_size/2)
      {
        ROS_DEBUG_NAMED ("nodes_on_grid", "On grid");
        res.first.push_back(n);
        res.second.push_back(local_pos);
      }
    }
  }

  return res;
  
}
コード例 #18
0
void RTKRobotArm::getEEPose(tf::Pose& pose) {
	if(mRobot) {
		//Collect ee pose from RTK
		MathLib::Vector eeq(4);
		Matrix3 tmp = mRobot->GetReferenceFrame(nEndEffectorId).GetOrient();
		tmp.GetQuaternionRepresentation(eeq);
		MathLib::Vector3 eep = mRobot->GetReferenceFrame(nEndEffectorId).GetOrigin();

		// Convert to tf
		pose.setRotation(tf::Quaternion(eeq[1], eeq[2], eeq[3], eeq[0]));
		pose.setOrigin(tf::Vector3(eep[0], eep[1], eep[2]));
	} else{
		ROS_WARN("RTK Robot not initialized");
	}
}
コード例 #19
0
ファイル: amcl_node.cpp プロジェクト: Renda110/AGVC
double
AmclNode::getYaw(tf::Pose& t)
{
    double yaw, pitch, roll;
    t.getBasis().getEulerYPR(yaw,pitch,roll);
    return yaw;
}
コード例 #20
0
void sendPose(tf::Pose& pose) {

	msg_pose.header.stamp = ros::Time::now();

	tf::Vector3 tmp = pose.getOrigin();
	msg_pose.pose.position.x = tmp.x();
	msg_pose.pose.position.y = tmp.y();
	msg_pose.pose.position.z = tmp.z();

	tf::Quaternion quat = pose.getRotation();
	msg_pose.pose.orientation.w = quat.w();
	msg_pose.pose.orientation.x = quat.x();
	msg_pose.pose.orientation.y = quat.y();
	msg_pose.pose.orientation.z = quat.z();

	pub_pose.publish(msg_pose);
}
コード例 #21
0
ファイル: amcl_node.cpp プロジェクト: SteveHuang27/navigation
double
AmclNode::getYaw(tf::Pose& t)
{
  double yaw, pitch, roll;
  btMatrix3x3 mat = t.getBasis();
  mat.getEulerYPR(yaw,pitch,roll);
  return yaw;
}
コード例 #22
0
ファイル: ahbros.hpp プロジェクト: andreasBihlmaier/ahbros
/**
 * Convert tf::Pose to string
 */
template<> std::string toString<tf::Pose>(const tf::Pose& p_pose)
{
  std::stringstream ss;

  ss << "Pose(Quaternion=" << toString(p_pose.getRotation()) << "; Vector3=" << toString(p_pose.getOrigin()) << ")";

  return ss.str();
}
コード例 #23
0
bool OdometryRemap::setOdomPoseCallback(nao_remote::SetTransform::Request& req, nao_remote::SetTransform::Response& res){
	ROS_INFO("New target for current odometry pose received");
	tf::Transform targetPose;
	tf::transformMsgToTF(req.offset, targetPose);

	m_odomOffset = targetPose * m_odomPose.inverse();

	return true;
}
コード例 #24
0
ファイル: motionmodel.cpp プロジェクト: iou16/3d_ogmapping
	tf::Pose MotionModel::drawFromMotion(tf::Pose& p) 
		{
			double sxy=0.3*srr;
			delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
			delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
			delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
			delta_yaw=fmod(delta_yaw, 2*M_PI);
			if (delta_yaw>M_PI)
				delta_yaw-=2*M_PI;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());

			return p;
		}
コード例 #25
0
	// Roll with "force" and horizontal "speed" until the length "range"
	bool rolling(double force, double speed, double range) {

		ROS_INFO_STREAM("Rolling with force "<<force<<", speed "<<speed<<", range "<<range);
		force = fabs(force);

		sendNormalForce(-force);
		msg_pose.pose.position.x  = ee_pose.getOrigin().x();
		msg_pose.pose.position.y  = ee_pose.getOrigin().y();
		msg_pose.pose.position.z  = ee_pose.getOrigin().z();

		tf::Quaternion q = ee_pose.getRotation();
		msg_pose.pose.orientation.x = q.x();
		msg_pose.pose.orientation.y = q.y();
		msg_pose.pose.orientation.z = q.z();
		msg_pose.pose.orientation.w = q.w();

		double center = ee_pose.getOrigin().y();
		double rate = 200;
		ros::Rate thread_rate(rate);
		int count=0;
		while(ros::ok()) {
			msg_pose.pose.position.y = msg_pose.pose.position.y + speed/rate;
			feedback_.progress = msg_pose.pose.position.y;
			as_.publishFeedback(feedback_);
			if( fabs(msg_pose.pose.position.y - center) >  range) {
				ROS_INFO("Change direction");
				speed *= -1;
				if(++count > 5) {
					break;
				}
			}
			pub_.publish(msg_pose);
			thread_rate.sleep();
		}

		msg_pose.pose.position.z = ee_pose.getOrigin().z() + 0.15;
		pub_.publish(msg_pose);
		sendNormalForce(0);

		return true;
	}
コード例 #26
0
	// Go to this pose
	bool go_home(tf::Pose& pose_) {


		tf::Vector3 start_p(pose_.getOrigin());
		tf::Quaternion start_o(pose_.getRotation());

		msg_pose.pose.position.x = start_p.x();
		msg_pose.pose.position.y = start_p.y();
		msg_pose.pose.position.z = start_p.z();
		msg_pose.pose.orientation.w = start_o.w();
		msg_pose.pose.orientation.x = start_o.x();
		msg_pose.pose.orientation.y = start_o.y();
		msg_pose.pose.orientation.z = start_o.z();
		pub_.publish(msg_pose);
		sendNormalForce(0);

		ros::Rate thread_rate(2);
		ROS_INFO("Homing...");
		while(ros::ok()) {
			double oerr =(ee_pose.getRotation() - start_o).length() ;
			double perr = (ee_pose.getOrigin() - start_p).length();
			feedback_.progress = 0.5*(perr+oerr);
			as_.publishFeedback(feedback_);
			ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
			if(perr< 0.02 && oerr < 0.02) {
				break;
			}
			if (as_.isPreemptRequested() || !ros::ok() )
			{
				sendNormalForce(0);
				sendPose(ee_pose);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			thread_rate.sleep();
		}
		return ros::ok();

	}
コード例 #27
0
gm::Point positionOnGrid (const unsigned n, const unsigned g, const Roadmap& r,
                          const tmap::TopologicalMap& tmap) 
{
  const msg::RoadmapNode& info = r.nodeInfo(n);
  const unsigned g2 = info.grid;
  if (g2==g)
  {
    return info.position;
  }
  else
  {
    const tmap::GraphVertex v = tmap.node(g);
    const tmap::GraphVertex v2 = tmap.node(g2);
    pair<tmap::GraphEdge, bool> res = edge(v, v2, tmap);
    ROS_ASSERT_MSG (res.second, "No edge between grid %u and grid %u "
                    "containing %u", g, g2, n);
    const bool flip = (tmap[res.first].dest == g);
    const tf::Pose offset = gmu::toPose(tmap[res.first].offset);
    const tf::Transform tr = flip ? offset.inverse() : offset;
    return gmu::transformPoint(tr, info.position);
  }
}
コード例 #28
0
ファイル: door_functions.cpp プロジェクト: Telpr2/pr2_doors
  double getNearestDoorAngle(const tf::Pose& robot_pose, const door_msgs::Door& door, double robot_dist, double touch_dist)
  { 
    double angle = 0, step = 0;
    if (door.rot_dir == door.ROT_DIR_CLOCKWISE)
      step = -0.01;
    else if (door.rot_dir == door.ROT_DIR_COUNTERCLOCKWISE)
      step = 0.01;
    else{
      ROS_ERROR("Door rot dir is not specified");
      return 0.0;
    }

    while ((robot_pose.getOrigin() - getGripperPose(door, angle, touch_dist).getOrigin()).length() < robot_dist &&
           fabs(angle) < M_PI_2)
      angle += step;

    return angle;
  }
コード例 #29
0
gs::LocalizationDistribution::Ptr OdomLocalizer::update (const Graph&, 
                                                         gs::LocalizationDistribution::ConstPtr localization,
                                                         const tf::Pose& rel_pose, const ScanPtr& scan)
{
  gs::LocalizationDistribution::Ptr dist(new gs::LocalizationDistribution());
  dist->stamp = scan->header.stamp;
  dist->samples.resize(1);
  ROS_ASSERT(localization->samples.size()==1);

  dist->samples[0].node = localization->samples[0].node;
  dist->samples[0].offset = util::absolutePose(localization->samples[0].offset, rel_pose);

  ROS_DEBUG_STREAM_COND_NAMED (fabs(rel_pose.getOrigin().x()) > 1e-3,
                               "localization", "In odom localization update with relative pose " <<
                               util::toString(rel_pose) << "; old loc was " << localization->samples[0] <<
                               " and new is " << dist->samples[0]);
  return dist;
}
コード例 #30
0
geometry_msgs::Twist PoseFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
{
    geometry_msgs::Twist res;
    tf::Pose diff = pose2.inverse() * pose1;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());

    if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
        return res;

    //in the case that we're not rotating to our goal position and we have a non-holonomic robot
    //we'll need to command a rotational velocity that will help us reach our desired heading

    //we want to compute a goal based on the heading difference between our pose and the target
    double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
                                  pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));


    //---------------Modified----------------------
    //we'll also check if we can move more effectively backwards
    //double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
    // pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));

    //check if its faster to just back up
//     if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
//       ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
//       yaw_diff = neg_yaw_diff;
//     }

    //compute the desired quaterion
    tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
    tf::Quaternion rot = pose2.getRotation() * rot_diff;
    tf::Pose new_pose = pose1;
    new_pose.setRotation(rot);

    diff = pose2.inverse() * new_pose;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());
    return res;
}