コード例 #1
0
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;
}
コード例 #2
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();
            }
        }
    }
コード例 #3
0
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);
}
コード例 #4
0
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;
}
コード例 #5
0
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;
}
コード例 #6
0
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();
}
コード例 #7
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()
                      );
  }
コード例 #8
0
// 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");
	}
}
コード例 #9
0
ファイル: ahbros.hpp プロジェクト: andreasBihlmaier/ahbros
/**
 * Convert tf::StampedTransform to string
 */
template<> std::string toString<tf::StampedTransform>(const tf::StampedTransform& p_transform)
{
  std::stringstream ss;

  ss << "StampedTransform(Quaternion=" << toString(p_transform.getRotation()) << "; Vector3=" << toString(p_transform.getOrigin()) << ")";

  return ss.str();
}
コード例 #10
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;

}
コード例 #11
0
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;
}
コード例 #12
0
ファイル: Actions.cpp プロジェクト: ChefOtter/SPQR_at_work
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;
  
}
コード例 #13
0
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";
}
コード例 #14
0
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);

}
コード例 #15
0
ファイル: tools.cpp プロジェクト: GuidoManfredi/essential
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;    
}
コード例 #16
0
ファイル: jlUtilities.cpp プロジェクト: jiayil/jiayi-ros-pkg
  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_;
  }
コード例 #17
0
ファイル: jlUtilities.cpp プロジェクト: jiayil/jiayi-ros-pkg
  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);
    
  }
コード例 #18
0
///////////////////////////////////////////////////////
/////-------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();
	}
}
コード例 #19
0
bool InputOutputLinControl::getRobotCommands(double displacement, tf::StampedTransform robot_pose, double k1, double k2, geometry_msgs::Pose poseB, geometry_msgs::Twist velB, double& linear_vel, double& angular_vel)
{
	double roll, pitch, yaw;
	robot_pose.getBasis().getRPY(roll,pitch,yaw);

	double a = cos(yaw);
	double b = sin(yaw);
	double c = -sin(yaw)/displacement;
	double d = cos(yaw)/displacement;

	double u1 = velB.linear.x + k1 * (poseB.position.x - robot_pose.getOrigin().getX());
	double u2 = velB.linear.y + k2 * (poseB.position.y - robot_pose.getOrigin().getY());

	//double u1 = k1 * (poseB.position.x - robot_pose.getOrigin().getX());
	//double u2 = k2 * (poseB.position.y - robot_pose.getOrigin().getY());

	linear_vel = a * u1 + b * u2;
	angular_vel = c * u1 + d * u2;

	geometry_msgs::PoseStamped poseR;
	poseR.header.frame_id = robot_pose.frame_id_;
	poseR.header.stamp = robot_pose.stamp_;
	poseR.pose.position.x = robot_pose.getOrigin().getX();
	poseR.pose.position.y = robot_pose.getOrigin().getY();
	poseR.pose.position.z = robot_pose.getOrigin().getZ();
	poseR.pose.orientation.x = robot_pose.getRotation().getX();
	poseR.pose.orientation.y = robot_pose.getRotation().getY();
	poseR.pose.orientation.z = robot_pose.getRotation().getZ();
	poseR.pose.orientation.w = robot_pose.getRotation().getW();
	ROS_INFO("traiettoria del robot x [%f]",robot_pose.getOrigin().getX());
	ROS_INFO("traiettoria del robot y [%f]",robot_pose.getOrigin().getY());
	local_path.poses.push_back(poseR);
	local_path_.publish(local_path);

	return true;

}
コード例 #20
0
	void MocapAlign::spinOnce( )
	{
		static tf::StampedTransform tf_a;
		static tf::StampedTransform tf_b;

		try
		{
			li.lookupTransform( frame_base, frame_a, ros::Time(0), tf_a );
			li.lookupTransform( frame_base, frame_b, ros::Time(0), tf_b );
		}
		catch( tf::TransformException ex )
		{
			ROS_INFO( "Missed a transform...chances are that we are still OK" );
			return;
		}
		if( tf_a.getOrigin( ).x( ) != tf_a.getOrigin( ).x( ) ||
			tf_b.getOrigin( ).x( ) != tf_b.getOrigin( ).x( ) )
		{
			ROS_WARN( "NaN DETECTED" );
			return;
		}

		// Rotate us to be aligned with the uav
		const tf::Quaternion delta_quat = tf::createQuaternionFromRPY( 0, 0, tf::getYaw( tf_a.getRotation( ) ) - tf::getYaw( tf_b.getRotation( ) ) );
		const tf::Quaternion quat_b_aligned = tf_b.getRotation( ) * delta_quat;

		// Broadcast the aligned tf
		tf::StampedTransform tf_b_aligned( tf_b );
		if( tf_b_aligned.getOrigin( ).x( ) != tf_b_aligned.getOrigin( ).x( ) )
		{
			ROS_WARN( "NaN PRODUCED" );
			return;
		}
		tf_b_aligned.setRotation( quat_b_aligned );
		tf_b_aligned.child_frame_id_ = tf_b_aligned.child_frame_id_ + "_aligned";
		br.sendTransform( tf_b_aligned );
	}
コード例 #21
0
Eigen::Matrix4d createHomogeneousTransformMatrix(tf::StampedTransform transform)
{
	tf::Point p = transform.getOrigin();
	tf::Quaternion q = transform.getRotation();
	tf::Matrix3x3 R1(q);
	Eigen::Matrix3d R2;
	tf::matrixTFToEigen(R1, R2);
	ROS_INFO_STREAM("R2:\n"<<R2);

	Eigen::Matrix4d T;
	T.block(0, 0, 3, 3) << R2;
	T.block(0, 3, 3, 1) << p.x(), p.y(), p.z();
	T.row(3) << 0, 0, 0, 1;
	return T;
}
コード例 #22
0
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());
}
コード例 #23
0
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());
}
コード例 #24
0
  void getPoseInRobotFrame(std::vector<SM_COND> nextRobotCond_w)
  { 
    //transforming position into robot frame
    geometry_msgs::PoseStamped goal;
    geometry_msgs::PoseStamped yawMapRobot;
    geometry_msgs::PoseStamped goalRobotFrame;

    // position
    goal.header.stamp= ros::Time(0);
    goal.header.frame_id= "map";
    goal.pose.position.x= nextRobotCond_w[0].x;
    goal.pose.position.y= nextRobotCond_w[1].x;
    goal.pose.position.z= 0.0;
    tf::Quaternion q = tf::createQuaternionFromRPY(0.0,0.0,nextRobotCond_w[5].x);
    tf::quaternionTFToMsg(q,goal.pose.orientation);
    
    _listener.waitForTransform("base_footprint", "map",
                               ros::Time(0), ros::Duration(1.0));
    _listener.transformPose("base_footprint", goal, goalRobotFrame); 
  
    _nextRobotCond_r[0].x = goalRobotFrame.pose.position.x;
    _nextRobotCond_r[1].x = goalRobotFrame.pose.position.y; 
    _nextRobotCond_r[5].x = tf::getYaw(goalRobotFrame.pose.orientation);

    // velocity and acceleration
    //record the starting transform from the odometry to the base frame
    _listener.lookupTransform("map", "base_footprint",
                              ros::Time(0), world_transform);
    
    double roll, pitch, yaw;
    q = world_transform.getRotation();
    btMatrix3x3(q).getRPY(roll, pitch, yaw);

    _nextRobotCond_r[0].v = cos(yaw)*nextRobotCond_w[0].v + sin(yaw)*nextRobotCond_w[1].v;
    _nextRobotCond_r[1].v = -sin(yaw)*nextRobotCond_w[0].v + cos(yaw)*nextRobotCond_w[1].v; 
    _nextRobotCond_r[5].v = nextRobotCond_w[5].v;

    _nextRobotCond_r[0].a = cos(yaw)*nextRobotCond_w[0].a + sin(yaw)*nextRobotCond_w[1].a;
    _nextRobotCond_r[1].a = -sin(yaw)*nextRobotCond_w[0].a + cos(yaw)*nextRobotCond_w[1].a;
    _nextRobotCond_r[5].a = nextRobotCond_w[5].a;
    
  }
コード例 #25
0
inline bool checkTF(tf::StampedTransform& transform, geometry_msgs::PoseStamped& robo,
					tf::TransformListener& listener)
{
	try{
		//listener.waitForTransform(header_frame, robot_frame, ros::Time(0), ros::Duration(1.0));
	    listener.lookupTransform(header_frame, robot_frame, ros::Time(0), transform);
        //copy robot position
        robo.pose.position.x=transform.getOrigin().x();
        robo.pose.position.y=transform.getOrigin().y();
        robo.pose.position.z=0;
        float angle = tf::getYaw(transform.getRotation());
        robo.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,angle);
		return true;
	}
	catch (tf::TransformException ex){
		// cout<<"waiting for global and robot frame relation"<<endl;
        ROS_ERROR("%s",ex.what());
		return false;
	}
}
コード例 #26
0
    void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
    {
        if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) )
        {
            try
            {
                // update the pose
                listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
                listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map);

                listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
                listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map);

                tf::Vector3 position_( kinect2map.getOrigin() );
                position.x() = position_.x();
                position.y() = position_.y();
                position.z() = position_.z();

                tf::Quaternion orientation_( kinect2map.getRotation() );
                orientation.x() = orientation_.x();
                orientation.y() = orientation_.y();
                orientation.z() = orientation_.z();
                orientation.w() = orientation_.w();

                ROS_INFO_STREAM("position = " << position.transpose() );
                ROS_INFO_STREAM("orientation = " << orientation.transpose() );

                // update the cloud
                pcl::copyPointCloud(*cloud, *xyz_cld_ptr);	// Do I need to copy it?
                //xyz_cld_ptr = cloud;
                cloud_updated = true;
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s", ex.what());
            }

        }
    }
コード例 #27
0
void addRead(string hexID, int rssi, tf::StampedTransform trans) {
    if (rssi == -1)  //The tags sometimes see each other; don't record this
        return;
    //Check to see if this tag has been seen yet
    if (heatmaps.find(hexID) == heatmaps.end()) {
        heatmaps[hexID] = PointCloud();
        heatmapPoses[hexID] = PoseArray();
        ChannelFloat32 rgbChannel;
        rgbChannel.name = "rgb";
        heatmaps[hexID].channels.push_back(rgbChannel);
        heatmaps[hexID].header.frame_id = "/map";
        heatmapSelected[hexID] = true;
    }
    double x = trans.getOrigin().x();
    double y = trans.getOrigin().y();
    double z = trans.getOrigin().z();
    Point32 P32;
    P32.x = x;P32.y = y;P32.z = z;
    Point P;
    P.x = x;P.y = y;P.z = z;
    btQuaternion rotation = trans.getRotation();
    Quaternion Q;
    Q.x = rotation.x();Q.y = rotation.y();Q.z = rotation.z();Q.w = rotation.w();
    Pose pose;
    pose.position = P;
    pose.orientation = Q;
    float float_rgb;
    if (rssi > 10) {
        double l = ((double)rssi + 50.0) / 200.0;
        int rgb = getColorHSL(20.0, 240.0, l, rssi);
        float_rgb = *reinterpret_cast<float*>(&rgb);
    }
    heatmaps[hexID].points.push_back(P32);
    //Color of point in RGB channel
    heatmaps[hexID].channels[0].values.push_back(float_rgb);
    heatmapPoses[hexID].poses.push_back(pose);
}
コード例 #28
0
ファイル: odom_tf_fncs.cpp プロジェクト: TuZZiX/coke_fetcher
geometry_msgs::PoseStamped OdomTf::get_pose_from_transform(tf::StampedTransform tf) {
    //clumsy conversions--points, vectors and quaternions are different data types in tf vs geometry_msgs
    geometry_msgs::PoseStamped stPose;
    geometry_msgs::Quaternion quat; //geometry_msgs object for quaternion
    tf::Quaternion tfQuat; // tf library object for quaternion
    tfQuat = tf.getRotation(); // member fnc to extract the quaternion from a transform
    quat.x = tfQuat.x(); // copy the data from tf-style quaternion to geometry_msgs-style quaternion
    quat.y = tfQuat.y();
    quat.z = tfQuat.z();
    quat.w = tfQuat.w();
    stPose.pose.orientation = quat; //set the orientation of our PoseStamped object from result

    // now do the same for the origin--equivalently, vector from parent to child frame 
    tf::Vector3 tfVec; //tf-library type
    geometry_msgs::Point pt; //equivalent geometry_msgs type
    tfVec = tf.getOrigin(); // extract the vector from parent to child from transform
    pt.x = tfVec.getX(); //copy the components into geometry_msgs type
    pt.y = tfVec.getY();
    pt.z = tfVec.getZ();
    stPose.pose.position = pt; //and use this compatible type to set the position of the PoseStamped
    stPose.header.frame_id = tf.frame_id_; //the pose is expressed w/rt this reference frame
    stPose.header.stamp = tf.stamp_; // preserve the time stamp of the original transform
    return stPose;
}
コード例 #29
0
void targetsCallback(const mtt::TargetList& list)
{
  //will print information that should be stored in a file
  //file format: id, good/bad tag, time, pos x, pos y, vel, theta
  //             position_diff, heading_diff, angle_to_robot, velocity_diff
  static ros::Time start_time = ros::Time::now();
  time_elapsed = ros::Time::now() - start_time;
  
  /// /// ROBOT PART //////
  //use transformations to extract robot features
  try{
    p_listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);
    p_listener->lookupTwist("/map", "/base_link", ros::Time(0), ros::Duration(0.5), twist);
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
  }

  robot_x = transform.getOrigin().x();
  
  robot_posex_buffer.push_back(robot_x);
  
  robot_y = transform.getOrigin().y();
  robot_theta = tf::getYaw(transform.getRotation());
  robot_vel = sqrt(pow(twist.linear.x, 2) + pow(twist.linear.y, 2));
   
  //robot output line 
  /// uncomment the following for training!
  printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,0,0,0,0\n",
         -1, leader_tag, time_elapsed.toSec(),
         robot_x, robot_y, robot_vel, robot_theta); 

  
  //testing new features extraction
//   accumulator_set<double, stats<tag::variance> > acc;
//   for_each(robot_posex_buffer.begin(), robot_posex_buffer.end(), boost::bind<void>(boost::ref(acc), _1));
//   printf("%f,%f,%f\n", robot_x, mean(acc), sqrt(variance(acc))); 
  
  /// /// TARGETS PART //////
  //sweeps target list and extract features
  for(uint i = 0; i < list.Targets.size(); i++){
    target_id = list.Targets[i].id;
    target_x = list.Targets[i].pose.position.x;
    target_y = list.Targets[i].pose.position.y;
    target_theta = tf::getYaw(list.Targets[i].pose.orientation);
    target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+
                      pow(list.Targets[i].velocity.linear.y,2));
    position_diff = sqrt(pow(robot_x - target_x,2)+
                        pow(robot_y - target_y,2));
    heading_diff = robot_theta - target_theta;
    angle_to_robot = -robot_theta + atan2(target_y - robot_y, 
                                        target_x - robot_x );
    velocity_diff = robot_vel - target_vel;
          
    //target output (to be used in adaboost training)
    
    // % output file format: 
    // % 1: id
    // % 2: good/bad tag
    // % 3: time
    // % 4: pos x
    // % 5: pos y
    // % 6: vel
    // % 7: theta
    // % 8: pos diff
    // % 9: head diff
    // %10: angle 2 robot 
    // %11: velocity diff 
    
    /// uncomment the following to generate training file!
    printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\n",
      target_id, leader_tag, time_elapsed.toSec(),
      target_x, target_y, target_vel, target_theta,
      position_diff, heading_diff, angle_to_robot, velocity_diff);
    
//     if(position_diff < 6.0 && target_vel > 0.5){
//       //if inside boundaries (in meters)
//       //store features in a covariance struct to send to matlab
//       nfeatures.pose.position.x = target_x;
//       nfeatures.pose.position.y = target_y;
//       nfeatures.pose.position.z = target_id;
// 
//       nfeatures.covariance[0] = target_vel;
//       nfeatures.covariance[1] = velocity_diff;
//       nfeatures.covariance[2] = heading_diff;
//       nfeatures.covariance[3] = angle_to_robot;
//       nfeatures.covariance[4] = position_diff;
//       
//       matlab_list.push_back(nfeatures);
// //       counter++;
//     }
  }
//   printf("targets within range: %d\n",counter);
//   counter = 0;
  
  //check if enough time has passed 
  //and send batch of msgs to matlab
//   duration_btw_msg = ros::Time::now() - time_last_msg;
//   
//   if(duration_btw_msg.toSec() > 0.01){
//     while(!matlab_list.empty()){
//       nfeatures_pub.publish(matlab_list.front());
//       matlab_list.pop_front();
//       usleep(0.01e6); //has to sleep, otherwise matlab do not get the msg
//     }      
//     time_last_msg = ros::Time::now();
//   }
}
コード例 #30
0
    /*
     * Marker type can be:
     *  0 - mesh
     *  1 - arrow
     */
    visualization_msgs::Marker makeMarker(
        const tf::StampedTransform & tf, std::string ns="marker", bool arrow=false)
    {
      unsigned int counter = 0;
      visualization_msgs::Marker marker;
      visualization_msgs::Marker textmarker;
      unsigned int shape;

      if (arrow == false)
        //shape = visualization_msgs::Marker::MESH_RESOURCE;
        shape = visualization_msgs::Marker::CYLINDER;
      else
        shape = visualization_msgs::Marker::ARROW;

      // Set the frame ID and timestamp.  See the TF tutorials for information on these.
      marker.header.frame_id = tf.frame_id_;

      marker.header.stamp = tf.stamp_;

      // 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 = ns;

      marker.id = counter;

      // 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
      marker.pose.position.x = tf.getOrigin().x();
      marker.pose.position.y = tf.getOrigin().y();
      marker.pose.position.z = tf.getOrigin().z();
      marker.pose.orientation.x = tf.getRotation().x();
      marker.pose.orientation.y = tf.getRotation().y();
      marker.pose.orientation.z = tf.getRotation().z();
      marker.pose.orientation.w = tf.getRotation().w();

      marker.scale.x = 1.0;
      marker.scale.y = 1.0;
      marker.scale.z = 1.0;

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

      if (arrow == false)
      {
        // Cylinder pivot point is center point, need another transform
        tf::Transform tf_cyl;
        tf_cyl.setOrigin(tf::Vector3(0.0, 0.0, (cyl_marker.h/2)));
        tf_cyl.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
        tf_cyl = tf * tf_cyl;
        marker.pose.position.x =    tf_cyl.getOrigin().x();
        marker.pose.position.y =    tf_cyl.getOrigin().y();
        marker.pose.position.z =    tf_cyl.getOrigin().z();
        marker.pose.orientation.x = tf_cyl.getRotation().x();
        marker.pose.orientation.y = tf_cyl.getRotation().y();
        marker.pose.orientation.z = tf_cyl.getRotation().z();
        marker.pose.orientation.w = tf_cyl.getRotation().w();
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;
        marker.color.a = 1.0f;
        marker.scale.x = marker.scale.y = (cyl_marker.r*2);
        marker.scale.z = cyl_marker.h;
      }

      marker.lifetime = marker_lifetime;

      return marker;
    }