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;
}
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();
}
}
}
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);
}
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;
}
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;
}
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();
}
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()
);
}
// 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");
}
}
/**
* 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();
}
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;
}
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 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";
}
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);
}
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 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_;
}
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);
}
///////////////////////////////////////////////////////
/////-------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();
}
}
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;
}
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 );
}
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;
}
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());
}
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;
}
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;
}
}
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());
}
}
}
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);
}
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;
}
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();
// }
}
/*
* 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;
}
