示例#1
0
void Robot::updateFrom(tf::TransformListener *tfListener) {
	using namespace Ogre;
	static tf::StampedTransform baseTF;
	static Vector3 translation = Vector3::ZERO;
	static Quaternion orientation = Quaternion::IDENTITY;
	static Quaternion qRot = Quaternion(-sqrt(0.5), 0.0f, sqrt(0.5), 0.0f)*Quaternion(-sqrt(0.5), sqrt(0.5), 0.0f, 0.0f);
	static Quaternion qYn90 = Quaternion(Degree(90), Vector3::NEGATIVE_UNIT_Y);
	static tfScalar yaw,pitch,roll;
	static Matrix3 mRot;
	
	try {
		tfListener->lookupTransform("map","base_footprint",ros::Time(0), baseTF);
		
		translation.x = baseTF.getOrigin().x();
		translation.y = baseTF.getOrigin().y();
		translation.z = baseTF.getOrigin().z();
		translation = qRot*translation + Vector3(0.0f, 1.0f, 0.0f);
		baseTF.getBasis().getEulerYPR(yaw,pitch,roll);
		mRot.FromEulerAnglesYXZ(Radian(yaw),Radian(0.0f),Radian(0.0f));
		orientation.FromRotationMatrix(mRot);
		orientation = qYn90*orientation;
		
		robot->setPosition(translation);
		robot->setOrientation(orientation);
		
	} catch (tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
	}
}
std::vector<desperate_housewife::fittedGeometriesSingle> HandPoseGenerator::SortbyHand(std::vector<desperate_housewife::fittedGeometriesSingle> objects_vec, tf::StampedTransform hand_position)
{

	geometry_msgs::Pose hand_position_local;
	hand_position_local.position.x = hand_position.getOrigin().x();
	hand_position_local.position.y = hand_position.getOrigin().y();
	hand_position_local.position.z = hand_position.getOrigin().z();
	hand_position_local.orientation.x = hand_position.getRotation().x();
	hand_position_local.orientation.y = hand_position.getRotation().y();
	hand_position_local.orientation.z = hand_position.getRotation().z();
	hand_position_local.orientation.w = hand_position.getRotation().w();

	Eigen::Matrix4d T_h_w = FromMsgtoEigen(hand_position_local);

	std::vector< desperate_housewife::fittedGeometriesSingle > objects_vec_roted_by_hand = objects_vec;

	for (unsigned int i = 0; i < objects_vec.size(); i++)
	{
		Eigen::Matrix4d T_o_w = FromMsgtoEigen(objects_vec_roted_by_hand[i].pose);
		geometry_msgs::Pose pose_temp;
		Eigen::Matrix4d T_o_h = T_h_w.inverse() * T_o_w;
		fromEigenToPose(T_o_h, pose_temp);
		// desperate_housewife::fittedGeometriesSingle object_temp = objects_vec[i];
		objects_vec_roted_by_hand[i].pose =  pose_temp;
		// objects_vec_roted_by_hand.push_back(object_temp);
	}

	std::sort(objects_vec_roted_by_hand.begin(), objects_vec_roted_by_hand.end(), [](desperate_housewife::fittedGeometriesSingle first, desperate_housewife::fittedGeometriesSingle second) {
		double distfirst = std::sqrt( first.pose.position.x * first.pose.position.x + first.pose.position.y * first.pose.position.y + first.pose.position.z * first.pose.position.z);
		double distsecond = std::sqrt( second.pose.position.x * second.pose.position.x + second.pose.position.y * second.pose.position.y + second.pose.position.z * second.pose.position.z);
		return (distfirst < distsecond);
	});

	return objects_vec_roted_by_hand;
}
gazebo::math::Pose gazebo::IAI_BoxDocking::tfToGzPose(tf::StampedTransform transform)
{
  math::Quaternion rotation = math::Quaternion( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z());
  math::Vector3 origin = math::Vector3(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); //T_q^w  
  math::Pose pose = math::Pose(origin,rotation);
  return pose;
}
示例#4
0
/*! \brief set the class attribut */
void Segment::setSegmentAttribut(tf::StampedTransform distal_transform_,tf::StampedTransform proximal_transform_,string segment_id_)
{
	this->proximal_joint.setValue(proximal_transform_.getOrigin().x(),proximal_transform_.getOrigin().y(),proximal_transform_.getOrigin().z());
	this->distal_joint.setValue(distal_transform_.getOrigin().x(),distal_transform_.getOrigin().y(),distal_transform_.getOrigin().z()) ;
	this->vec_seg 	   = -(this->proximal_joint - this->distal_joint);
	this->vec_uni 	   = this->vec_seg.normalized() ;
	if(this->vec_uni.getX() != 0)   this->p_limite = this->vec_seg.getX()/this->vec_uni.getX();
	this->segment_id = segment_id_;	
	//-------------------------> affichage des variables du segment
/*
	std::cout <<"prox_x =" << proximal_transform_.getOrigin().x() << std::endl;	
	std::cout <<"prox_y =" << proximal_transform_.getOrigin().y() << std::endl;	
	std::cout <<"prox_z =" << proximal_transform_.getOrigin().z() << std::endl;	

	std::cout <<"dist_x =" << distal_transform_.getOrigin().x() << std::endl;	
	std::cout <<"dist_y =" << distal_transform_.getOrigin().y() << std::endl;	
	std::cout <<"dist_z =" << distal_transform_.getOrigin().z() << std::endl;

	std::cout <<"vec_seg = (" << vec_seg.x() << ";" << vec_seg.y() << ";" << vec_seg.z() << ")" << std::endl;	
	std::cout <<"vec_uni = (" << vec_uni.x() << ";" << vec_uni.y() << ";" << vec_uni.z() << ")" << std::endl;	
	std::cout <<"p_limite =" << this->vec_seg.getX()/this->vec_uni.getX() << std::endl;	
	std::cout <<"segment_id =" << segment_id << std::endl;
*/
	//------------------------------------------------------------------------------------------------------------
}
示例#5
0
  void computeFrustum()
  {
    // compute frustum based on camera info and tf
    const double alphaY = cameraInfo.K[4];
    const double fovY = 2 * atan(cameraInfo.height / (2 * alphaY));
    tf::Vector3 up(0, 1, 0), focal(1, 0, 0);
    up = camToWorld * up;
    focal = camToWorld * focal;

    camera.fovy = fovY;
    camera.clip[0] = 0.001;
    camera.clip[1] = 12.0;
    camera.window_size[0] = cameraInfo.width;
    camera.window_size[1] = cameraInfo.height;
    camera.window_pos[0] = 0;
    camera.window_pos[1] = 0;
    camera.pos[0] = worldToCam.getOrigin().x();
    camera.pos[1] = worldToCam.getOrigin().y();
    camera.pos[2] = worldToCam.getOrigin().z();
    camera.view[0] = up.x();
    camera.view[1] = up.y();
    camera.view[2] = up.z();
    camera.focal[0] = focal.x();
    camera.focal[1] = focal.y();
    camera.focal[2] = focal.z();

    Eigen::Matrix4d viewMatrix;
    Eigen::Matrix4d projectionMatrix;
    Eigen::Matrix4d viewProjectionMatrix;
    camera.computeViewMatrix(viewMatrix);
    camera.computeProjectionMatrix(projectionMatrix);
    viewProjectionMatrix = projectionMatrix * viewMatrix;
    pcl::visualization::getViewFrustum(viewProjectionMatrix, frustum);
  }
void InputOutputLinControl::getRelativeTransformation2D(tf::StampedTransform v1, tf::StampedTransform v2, tf::Transform& relative_transform)
{
	double roll, pitch, yaw1, yaw2;
	v1.getBasis().getRPY(roll,pitch,yaw1);
	v2.getBasis().getRPY(roll,pitch,yaw2);

	double a11 = cos(yaw1 - yaw2);
	double a12 = sin(yaw1 - yaw2);
	double a13 = 0;

	double a21 = - a12;
	double a22 = a11;
	double a23 = 0;

	double a31 = 0;
	double a32 = 0;
	double a33 = 1;

	double t1 = v2.getOrigin().getX() - v1.getOrigin().getX() * a11 - v1.getOrigin().getY()*a12;
	double t2 = v2.getOrigin().getY() - v1.getOrigin().getY() * a11 + v1.getOrigin().getX()*a12;
	double t3 = 0;

	relative_transform = tf::Transform(btMatrix3x3(a11,a12,a13,a21,a22,a23,a31,a32,a33),btVector3(t1,t2,t3));

}
std::vector<desperate_housewife::fittedGeometriesSingle> HandPoseGenerator::transform_to_world(std::vector<desperate_housewife::fittedGeometriesSingle> objects_vec, tf::StampedTransform hand_position)
{

	geometry_msgs::Pose hand_position_local;
	hand_position_local.position.x = hand_position.getOrigin().x();
	hand_position_local.position.y = hand_position.getOrigin().y();
	hand_position_local.position.z = hand_position.getOrigin().z();
	hand_position_local.orientation.x = hand_position.getRotation().x();
	hand_position_local.orientation.y = hand_position.getRotation().y();
	hand_position_local.orientation.z = hand_position.getRotation().z();
	hand_position_local.orientation.w = hand_position.getRotation().w();

	Eigen::Matrix4d T_h_w = FromMsgtoEigen(hand_position_local);

	std::vector< desperate_housewife::fittedGeometriesSingle > objects_vec_roted_by_hand = objects_vec;

	for (unsigned int p = 0; p < objects_vec_roted_by_hand.size(); p ++)
	{
		geometry_msgs::Pose pose_temp;
		Eigen::Matrix4d T_o_h = FromMsgtoEigen(objects_vec_roted_by_hand[p].pose);
		Eigen::Matrix4d Temp = T_h_w * T_o_h;
		fromEigenToPose(Temp, pose_temp);
		objects_vec_roted_by_hand[p].pose = pose_temp;
	}

	return objects_vec_roted_by_hand;
}
void InputOutputLinControl::markerFromPose(std::string name_space, double red, double green, double blue, tf::StampedTransform pose, visualization_msgs::Marker& marker)
{
	marker.header.frame_id = pose.frame_id_;
	marker.header.stamp = pose.stamp_;
	marker.ns = name_space.c_str();
	marker.id = 0;
	marker.type = visualization_msgs::Marker::ARROW;
	marker.action = visualization_msgs::Marker::ADD;

	marker.pose.position.x = pose.getOrigin().getX();
	marker.pose.position.y = pose.getOrigin().getY();
	marker.pose.position.z = pose.getOrigin().getZ();
	marker.pose.orientation.x = pose.getRotation().getX();
	marker.pose.orientation.y = pose.getRotation().getY();
	marker.pose.orientation.z = pose.getRotation().getZ();
	marker.pose.orientation.w = pose.getRotation().getW();

	marker.scale.x = 1;
	marker.scale.y = 1;
	marker.scale.z = 0.8;

	marker.color.a = 1;
	marker.color.g = green;
	marker.color.r = red;
	marker.color.b = blue;

	marker.lifetime = ros::Duration(0);
}
    geometry_msgs::PoseStamped getCartBaseLinkPosition()
    {
        geometry_msgs::PoseStamped pose_base_link;
        pose_base_link.header.frame_id = base_link_;

        while (nh_.ok()) {
            try {
                tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);

                pose_base_link.pose.position.x = base_link_transform_.getOrigin().x();
                pose_base_link.pose.position.y = base_link_transform_.getOrigin().y();
                pose_base_link.pose.position.z = base_link_transform_.getOrigin().z();
                pose_base_link.pose.orientation.x = base_link_transform_.getRotation().x();
                pose_base_link.pose.orientation.y = base_link_transform_.getRotation().y();
                pose_base_link.pose.orientation.z = base_link_transform_.getRotation().z();
                pose_base_link.pose.orientation.w = base_link_transform_.getRotation().w();

                return pose_base_link;
            }
            catch (tf::TransformException ex){
                ROS_ERROR("%s",ex.what());
                ros::Duration(0.1).sleep();
            }
        }
    }
示例#10
0
/* ! \brief with transforms without segment_id */
Segment::Segment(tf::StampedTransform proximal_transform_,tf::StampedTransform distal_transform_)
{
	this->proximal_joint.setValue(proximal_transform_.getOrigin().x(),proximal_transform_.getOrigin().y(),proximal_transform_.getOrigin().z());
	this->distal_joint.setValue(distal_transform_.getOrigin().x(),distal_transform_.getOrigin().y(),distal_transform_.getOrigin().z()) ;
	this->vec_seg 	   = -(proximal_joint-distal_joint);
	this->vec_uni 	   = this->vec_seg.normalized() ;
	if(this->vec_uni.getX() != 0)   this->p_limite = this->vec_seg.getX()/this->vec_uni.getX();


}
示例#11
0
cv::Mat transform2mat (tf::StampedTransform transform) {
    double x = transform.getOrigin().x();
    double y = transform.getOrigin().y();
    double z = transform.getOrigin().z();
    tf::Matrix3x3 R(transform.getRotation());
    Mat P = (Mat_<float>(4,4) << R[0][0], R[0][1], R[0][2], x,
                                 R[1][0], R[1][1], R[1][2], y, 
                                 R[2][0], R[2][1], R[2][2], z,
                                 0, 0, 0, 1);
    return P;    
}
void transform_callback(tf::StampedTransform transform){
	double roll,pitch,yaw;

	Pos_Controller_U.position[0]=transform.getOrigin().x();
	Pos_Controller_U.position[1]=transform.getOrigin().y();
	Pos_Controller_U.position[2]=transform.getOrigin().z();
	tf::Matrix3x3(transform.getRotation()).getRPY(roll,pitch,yaw);
	Pos_Controller_U.yaw=yaw;
	ROS_INFO("new position : %f %f %f %f",Pos_Controller_U.position[0],Pos_Controller_U.position[1],Pos_Controller_U.position[2], yaw);

}
void InputOutputLinControl::updateMarkerFromPose(tf::StampedTransform pose, visualization_msgs::Marker& marker)
{
	marker.header.stamp = pose.stamp_;
	marker.header.frame_id = pose.frame_id_;
	marker.pose.position.x = pose.getOrigin().getX();
	marker.pose.position.y = pose.getOrigin().getY();
	marker.pose.position.z = pose.getOrigin().getZ();
	marker.pose.orientation.x = pose.getRotation().getX();
	marker.pose.orientation.y = pose.getRotation().getY();
	marker.pose.orientation.z = pose.getRotation().getZ();
	marker.pose.orientation.w = pose.getRotation().getW();
}
示例#14
0
  void convertTFtoVispHMat(const tf::StampedTransform &TFtransform,
                           vpHomogeneousMatrix &HMatrix)
  {
    double Angle; // Theta U angle of the transform
    Angle = TFtransform.getRotation().getAngle();

    HMatrix.buildFrom(TFtransform.getOrigin().x(),
                      TFtransform.getOrigin().y(),
                      TFtransform.getOrigin().z(),
                      Angle*TFtransform.getRotation().getAxis().x(),
                      Angle*TFtransform.getRotation().getAxis().y(),
                      Angle*TFtransform.getRotation().getAxis().z()
                      );
  }
示例#15
0
// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
  Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
  Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
  ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);

  // setup init map
  bool init_set_map[MAP_CELLS];
  for (int i=0; i<MAP_CELLS;i++) {
    init_set_map[i] = false;
  }
  setVal(init_set_map,goal_pt.x,goal_pt.y,true);

  //updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
  LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}
示例#16
0
	bool poseTracker::newPose(tf::StampedTransform transform){
		
		Eigen::Vector3f curr(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
		pose.push(curr);
		if(pose.size() < 50){
			return true;
		}

		float squaredNorm = (pose.front() - curr).squaredNorm();
		pose.pop();
		bool tmp = squaredNorm > 0.2;
		std::cout << tmp << "  Max: " << squaredNorm << std::endl;
		return squaredNorm > 0.2;

	}
// Calculating and returning the center2center transform which belongs to stampedT_in
// [ A(front), B(right), C(back) or D(left) ]
tf::StampedTransform C2C_LEFT_Node::get_c2c(const tf::StampedTransform& stampedT_in)
{
	tf::Transform T_in = tf::Transform(stampedT_in.getRotation(), stampedT_in.getOrigin());

	if (stampedT_in.child_frame_id_[1] != '_')		// if stampedT_in was stand-alone (previously 1 transforms)
	{
		switch (stampedT_in.child_frame_id_[1] - '0')
		{
			case 0: case 1:	return tf::StampedTransform(apollon[1]*T_in*boreas[3].inverse(), ros::Time::now(), "apollon_center", "A_" + side + "_c2c");
			case 2: case 3:	return tf::StampedTransform(apollon[1]*T_in*boreas[4].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "B_" + side + "_c2c");
			case 4: case 5:	return tf::StampedTransform(apollon[1]*T_in*boreas[5].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "C_" + side + "_c2c");
			case 6: case 7:	return tf::StampedTransform(apollon[1]*T_in*boreas[6].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "D_" + side + "_c2c");
		}
	}

	else		// else stampedT_in was merged (previously 2 transforms)
	{
		if (stampedT_in.child_frame_id_[0] == 'A')
			return tf::StampedTransform(apollon[1]*T_in*boreas[3].inverse(), ros::Time::now(), "apollon_center", "A_" + side + "_c2c");
		else if (stampedT_in.child_frame_id_[0] == 'B')
			return tf::StampedTransform(apollon[1]*T_in*boreas[4].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "B_" + side + "_c2c");
		else if (stampedT_in.child_frame_id_[0] == 'C')
			return tf::StampedTransform(apollon[1]*T_in*boreas[5].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "C_" + side + "_c2c");
		else if (stampedT_in.child_frame_id_[0] == 'D')
			return tf::StampedTransform(apollon[1]*T_in*boreas[6].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "D_" + side + "_c2c");
	}
}
示例#18
0
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
  size_t size = pointCloud.points.size();
  
  dataContainer.clear();
  
  tf::Vector3 laserPos (laserTransform.getOrigin());
  dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
  
  for (size_t i = 0; i < size; ++i)
    {
      
      const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
      
      float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
      
      if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
	
	if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
	  continue;
	}
	
	tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
	
	float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
	
	if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
	  {
	    dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
	  }
      }
    }
  
  return true;
}
示例#19
0
bool
WmObjectCapture::updateObjectFrame(const ros::Time& stamp, tf::StampedTransform& m2c)
{
	std::vector<tf::StampedTransform> valids;

	getValidMarks(valids, stamp);
	if(valids.size()<4)
	{
		//ROS_INFO("No enough marks detected [%zu]", valids.size());
		return false;
	}

	tf::Transform media;
	double stdev;
	getBestTransform(valids, media, stdev);

	double m_tolerance2 = 0.006*0.006;
	if(stdev<m_tolerance2)
	{
		//ROS_INFO("TRANSFORM ACCEPTED ");
		m2c.child_frame_id_ = objectFrameId_;
		m2c.frame_id_ = cameraFrameId_;
		m2c.stamp_ = ros::Time::now();

		m2c.setOrigin(media.getOrigin());
		m2c.setRotation(media.getRotation());

		ROS_DEBUG("NEW FINAL (%lf, %lf, %lf) (%lf, %lf, %lf, %lf)",
				m2c.getOrigin().x(), m2c.getOrigin().y(), m2c.getOrigin().z(),
				m2c.getRotation().x(), m2c.getRotation().y(), m2c.getRotation().z(), m2c.getRotation().w());

		try
		{
			tfBroadcaster_.sendTransform(m2c);

		}catch(tf::TransformException & ex)
		{
			ROS_WARN("WmObjectCapture::updateObjectFrame %s",ex.what());
		}
		return true;
	}

	//ROS_INFO("TRANSFORM REJECTED ");
	return false;

}
    bool spawnObject(){
        try{
            ros::Time time = ros::Time::now();
            tf_listener.waitForTransform("table_top", "object", time, ros::Duration(2.0));
            tf_listener.lookupTransform("table_top", "object", time, transform);
        }catch(tf::TransformException ex){
            return false;
        }

        objectHeight = transform.getOrigin().getZ();
        moveit_msgs::ApplyPlanningScene srv;
        moveit_msgs::PlanningScene planning_scene;
        planning_scene.is_diff = true;

        moveit_msgs::CollisionObject object;
        object.header.frame_id = "object";
        object.id = "can";

        shape_msgs::SolidPrimitive primitive;
        primitive.type = primitive.CYLINDER;
        primitive.dimensions.push_back(objectHeight); //height
        primitive.dimensions.push_back(0.04); //radius
        object.primitives.push_back(primitive);

        geometry_msgs::Pose pose;
        pose.orientation.w = 1.0;
        pose.position.z = -objectHeight/2;
        object.primitive_poses.push_back(pose);

        place_pose.header.frame_id = "table_top";
        place_pose.pose.orientation.x = 0.5;
        place_pose.pose.orientation.y = 0.5;
        place_pose.pose.orientation.z = -0.5;
        place_pose.pose.orientation.w = 0.5;
        place_pose.pose.position.x = transform.getOrigin().getX();
        place_pose.pose.position.y = transform.getOrigin().getY();
        place_pose.pose.position.z = transform.getOrigin().getZ() + 0.005;

        object.operation = object.ADD;
        planning_scene.world.collision_objects.push_back(object);

        srv.request.scene = planning_scene;
        planning_scene_diff_client.call(srv);

        return true;
    }
void InputOutputLinControl::getTrueRobotPose(double displacement, tf::StampedTransform robot_poseB, tf::StampedTransform& true_robot_pose)
{
	true_robot_pose.frame_id_ = robot_poseB.frame_id_;
	true_robot_pose.stamp_ = robot_poseB.stamp_;

	double roll, pitch, yaw;
	robot_poseB.getBasis().getRPY(roll,pitch,yaw);

	tf::Vector3 v;
	v.setX(robot_poseB.getOrigin().getX() - displacement*cos(yaw));
	v.setY(robot_poseB.getOrigin().getY() - displacement*sin(yaw));
	v.setZ(robot_poseB.getOrigin().getZ());

	true_robot_pose.setOrigin(v);

	true_robot_pose.setRotation(robot_poseB.getRotation());
}
void InputOutputLinControl::realRobotPoseB(double displacement, tf::StampedTransform real_robot_pose, tf::StampedTransform& real_robot_poseB)
{
	real_robot_poseB.frame_id_ = real_robot_pose.frame_id_;
	real_robot_poseB.stamp_ = real_robot_pose.stamp_;
	real_robot_poseB.child_frame_id_ = real_robot_pose.child_frame_id_;

	tf::Vector3 v;
	double roll, pitch, yaw;
	real_robot_pose.getBasis().getRPY(roll,pitch,yaw);

	v.setX(real_robot_pose.getOrigin().getX() + displacement*cos(yaw));
	v.setY(real_robot_pose.getOrigin().getY() + displacement*sin(yaw));
	v.setZ(real_robot_pose.getOrigin().getZ());
	real_robot_poseB.setOrigin(v);

	real_robot_poseB.setRotation(real_robot_pose.getRotation());
}
geometry_msgs::Pose convertToPose(tf::StampedTransform transform)
{
	tf::Quaternion q = transform.getRotation();
	tf::Point p = transform.getOrigin();

	geometry_msgs::Pose pose;
	pose.position = createPoint(p.x(), p.y(), p.z());
	pose.orientation = createQuaternion(q.x(), q.y(), q.z(), q.w());
	return pose;
}
void StigmergyPlanner::makeViewFacingMarker(tf::StampedTransform robot_pose)
{
  int_marker.header.frame_id = robot_pose.frame_id_;
  int_marker.header.stamp = robot_pose.stamp_;

  int_marker.pose.position.x = robot_pose.getOrigin().getX();
  int_marker.pose.position.y = robot_pose.getOrigin().getY();
  int_marker.pose.position.z = robot_pose.getOrigin().getZ();
  int_marker.scale = 1;

  int_marker.name = "view_facing";
  int_marker.description = "3D Planning pencil";

  InteractiveMarkerControl control;

  // make a control that rotates around the view axis
  //control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  //control.orientation.w = 1;
  //control.name = "rotate";

  //int_marker.controls.push_back(control);

  // create a box in the center which should not be view facing,
  // but move in the camera plane.
  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
  control.independent_marker_orientation = true;
  control.name = "move";

  control.markers.push_back( makeBox(int_marker) );
  control.always_visible = true;

  int_marker.controls.push_back(control);

  control.interaction_mode = InteractiveMarkerControl::MENU;
  control.name = "menu";
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, boost::bind(&StigmergyPlanner::processFeedback,this,_1));
  menu_handler.apply(*server, int_marker.name);
}
示例#25
0
void RockinPNPActionServer::tf2Affine(tf::StampedTransform& tf, Eigen::Affine3d& T)
{
  tf::Vector3 o=tf.getOrigin();
  tf::Quaternion q_tf=tf.getRotation();
  Eigen::Quaterniond q(q_tf[3],q_tf[0],q_tf[1],q_tf[2]);
  Eigen::Matrix3d R(q);
  Eigen::Vector3d t(o[0],o[1],o[2]);
  T.linear()=R; T.translation()=t;
  
}
void printTF(tf::StampedTransform t, WhichArm a)
{
	std::string p;
	if(a==RIGHT)
	{
		std::cout<< "#Right gripper:\n";
		p="r";
	}
	else
	{
		std::cout<< "#Left gripper:\n";
		p="l";
	}

	std::cout<< "p.position.x     = " << t.getOrigin().getX()    << ";\n"
			 << "p.position.y     = " << t.getOrigin().getY()    << ";\n"
			 << "p.position.z     = " << t.getOrigin().getZ()    << ";\n"
			 << "p.orientation.x  = " << t.getRotation().getX()  << ";\n"
			 << "p.orientation.y  = " << t.getRotation().getY()  << ";\n"
			 << "p.orientation.z  = " << t.getRotation().getZ()  << ";\n"
			 << "p.orientation.w  = " << t.getRotation().getW()  << ";\n";

	geometry_msgs::PoseStamped tmp;
	tmp.pose.position.x 	= t.getOrigin().getX()  ;
	tmp.pose.position.y     = t.getOrigin().getY()  ;
	tmp.pose.position.z     = t.getOrigin().getZ()  ;
	tmp.pose.orientation.x 	= t.getRotation().getX();
	tmp.pose.orientation.y  = t.getRotation().getY();
	tmp.pose.orientation.z  = t.getRotation().getZ();
	tmp.pose.orientation.w  = t.getRotation().getW();
	std::cout<< "    pose:\n"
			 << "      position:\n"
			 << "        x: "			<< tmp.pose.position.x << "\n"
			 << "        y: "			<< tmp.pose.position.y << "\n"
			 << "        z: "			<< tmp.pose.position.z << "\n"
			 << "      orientation:\n"
			 << "        x: "			<< tmp.pose.orientation.x << "\n"
			 << "        y: "			<< tmp.pose.orientation.y << "\n"
			 << "        z: "			<< tmp.pose.orientation.z << "\n"
			 << "        w: "			<< tmp.pose.orientation.w << "\n\n";
//	std::cout<<tmp<<"\n";
}
示例#27
0
  void tfToPose(tf::StampedTransform &stampedTF, geometry_msgs::PoseStamped &msg)
  {
      tf::Vector3 translation = stampedTF.getOrigin();

      msg.pose.position.x = translation.x();
      msg.pose.position.y = translation.y();
      msg.pose.position.z = translation.z();

      tf::quaternionTFToMsg(stampedTF.getRotation(), msg.pose.orientation);
      msg.header.stamp = stampedTF.stamp_;
      msg.header.frame_id = stampedTF.frame_id_;
  }
示例#28
0
  void tfToPose(tf::StampedTransform &stampedTF, geometry_msgs::Pose &msg)
  {

    
    tf::Vector3 translation = stampedTF.getOrigin();
    
    msg.position.x = translation.x();
    msg.position.y = translation.y();
    msg.position.z = translation.z();
    
    tf::quaternionTFToMsg(stampedTF.getRotation(), msg.orientation);
    
  }
bool InputOutputLinControl::updateRobotPoseByModel(double timestep, double linear_vel, double angular_vel, tf::StampedTransform& pose)
{

	pose.stamp_ = ros::Time::now();
	double roll, pitch, yaw;
	pose.getBasis().getRPY(roll,pitch,yaw);

	tf::Vector3 v;

	double theta = yaw + angular_vel * timestep;

	//Eulerian integration
	v.setX(pose.getOrigin().getX() + linear_vel * timestep * cos(theta));
	v.setY(pose.getOrigin().getY() + linear_vel * timestep * sin(theta));
	v.setZ(pose.getOrigin().getZ());

	pose.setOrigin(v);
	pose.setRotation(tf::createQuaternionFromYaw(theta));
	ROS_INFO("New orientation [%f]",theta);

	return true;
}
///////////////////////////////////////////////////////
/////-------Cordinate Convert function---------////////
///////////////////////////////////////////////////////
void pathLocal2Global(nav_msgs::Path& path, const tf::StampedTransform transform)
{
	int length=path.poses.size();
	nav_msgs::Odometry zero;
	float angle = tf::getYaw(transform.getRotation()) - 0;
	for(int i=0;i<length;i++){
		float tmp_x = path.poses[i].pose.position.x - zero.pose.pose.position.x;
		float tmp_y = path.poses[i].pose.position.y - zero.pose.pose.position.y;
		float conv_x = cos(angle)*tmp_x - sin(angle)*tmp_y;
		float conv_y = sin(angle)*tmp_x + cos(angle)*tmp_y;
		path.poses[i].pose.position.x = conv_x + transform.getOrigin().x();
		path.poses[i].pose.position.y = conv_y + transform.getOrigin().y();
	}
}