void save_localize_transform() { std::ofstream filed_transform; std::stringstream file_name; std::string path = ros::package::getPath("prx_localizer"); file_name << path << "/transform/localize_transform.bin"; filed_transform.open(file_name.str().c_str(), std::ofstream::binary|std::ofstream::trunc); tfScalar x, y, z, w; x = g_localize_transform.getRotation().getX(); y = g_localize_transform.getRotation().getY(); z = g_localize_transform.getRotation().getZ(); w = g_localize_transform.getRotation().getW(); filed_transform.write((char *) &x, sizeof(tfScalar)); filed_transform.write((char *) &y, sizeof(tfScalar)); filed_transform.write((char *) &z, sizeof(tfScalar)); filed_transform.write((char *) &w, sizeof(tfScalar)); x = g_localize_transform.getOrigin().getX(); y = g_localize_transform.getOrigin().getY(); z = g_localize_transform.getOrigin().getZ(); filed_transform.write((char *) &x, sizeof(tfScalar)); filed_transform.write((char *) &y, sizeof(tfScalar)); filed_transform.write((char *) &z, sizeof(tfScalar)); filed_transform.close(); }
inline tf::Transform interpolateTF(tf::Transform start, tf::Transform end, double ratio) { tf::Vector3 lerp_pos = start.getOrigin().lerp(end.getOrigin(), ratio); tf::Quaternion lerp_rot = start.getRotation().slerp(end.getRotation(), ratio); return tf::Transform(lerp_rot, lerp_pos); }
void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){ sensor_msgs::PointCloud2 cloud_in,cloud_out; projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_); rot_tf.header.frame_id = "/ChestLidar"; // lidar mx28 axis aligned mode // rot_tf.transform.rotation.x = enc_tf_.getRotation().x(); // rot_tf.transform.rotation.y = enc_tf_.getRotation().y(); // rot_tf.transform.rotation.z = enc_tf_.getRotation().z(); // rot_tf.transform.rotation.w = enc_tf_.getRotation().w(); // lidar mx28 axis perpendicular modeg tf::Quaternion q1; q1.setRPY(-M_PI/2,0,0); tf::Transform m1(q1); tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w()); tf::Transform m2(q2); tf::Transform m4; m4 = m2*m1; // rotate lidar axis and revolute mx28 axis rot_tf.transform.rotation.x = m4.getRotation().x(); rot_tf.transform.rotation.y = m4.getRotation().y(); rot_tf.transform.rotation.z = m4.getRotation().z(); rot_tf.transform.rotation.w = m4.getRotation().w(); tf2::doTransform(cloud_in,cloud_out,rot_tf); point_cloud_pub_.publish(cloud_out); moving_now.data = dxl_ismove_; dxl_ismove_pub_.publish(moving_now); }
void fromTF(const tf::Transform &source, Eigen::Matrix4f &dest, geometry_msgs::Pose &pose_dest) { Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ()); q.normalize(); Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z()); Eigen::Matrix3f R(q.toRotationMatrix()); dest(0,0) = R(0,0); dest(0,1) = R(0,1); dest(0,2) = R(0,2); dest(1,0) = R(1,0); dest(1,1) = R(1,1); dest(1,2) = R(1,2); dest(2,0) = R(2,0); dest(2,1) = R(2,1); dest(2,2) = R(2,2); dest(3,0) = dest(3,1)= dest(3,2) = 0; dest(3,3) = 1; dest(0,3) = t(0); dest(1,3) = t(1); dest(2,3) = t(2); pose_dest.orientation.x = q.x(); pose_dest.orientation.y = q.y(); pose_dest.orientation.z = q.z(); pose_dest.orientation.w = q.w(); pose_dest.position.x = t(0); pose_dest.position.y = t(1); pose_dest.position.z = t(2); }
std::vector<double> planning_models::KinematicModel::PlanarJointModel::computeJointStateValues(const tf::Transform& transform) const { std::vector<double> ret; ret.push_back(transform.getOrigin().x()); ret.push_back(transform.getOrigin().y()); ret.push_back(transform.getRotation().getAngle()*transform.getRotation().getAxis().z()); return ret; }
void leatherman::btTransformToPoseMsg(const tf::Transform &bt, geometry_msgs::Pose &pose) { pose.position.x = bt.getOrigin().getX(); pose.position.y = bt.getOrigin().getY(); pose.position.z = bt.getOrigin().getZ(); pose.orientation.x = bt.getRotation().x(); pose.orientation.y = bt.getRotation().y(); pose.orientation.z = bt.getRotation().z(); pose.orientation.w = bt.getRotation().w(); }
void printTransform(tf::Transform& t){ for(int i=0; i<3; i++){ std::cout << t.getOrigin()[i] << "\t"; } std::cout << t.getRotation()[3] << "\t"; for(int i=0; i<3; i++){ std::cout << t.getRotation()[i] << "\t"; } std::cout << std::endl; }
void fromTF(const tf::Transform &source, geometry_msgs::Pose &dest) { dest.orientation.x = source.getRotation().getX(); dest.orientation.y = source.getRotation().getY(); dest.orientation.z = source.getRotation().getZ(); dest.orientation.w = source.getRotation().getW(); dest.position.x = source.getOrigin().x(); dest.position.y = source.getOrigin().y(); dest.position.z = source.getOrigin().z(); }
//FIXME //calculate the absolute orientation of the object // float getOrient(tf::Transform & pose1, tf::Transform & pose2) { float orient; /* The orientation is the angle of (x-axis) rotation about the z-axis, * under the assumption that z-axis does not rotate on x or y */ orient = acos( sqrt(pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getY(),2)) / sqrt(pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getY(),2)) ); return orient; }
geometry_msgs::PoseWithCovarianceStamped tfToPoseCovarianceStamped (const tf::Transform &pose) { geometry_msgs::PoseWithCovarianceStamped pocv; pocv.pose.pose.position.x = pose.getOrigin().x(); pocv.pose.pose.position.y = pose.getOrigin().y(); pocv.pose.pose.position.z = pose.getOrigin().z(); pocv.pose.pose.orientation.x = pose.getRotation().x(); pocv.pose.pose.orientation.y = pose.getRotation().y(); pocv.pose.pose.orientation.z = pose.getRotation().z(); pocv.pose.pose.orientation.w = pose.getRotation().w(); return pocv; }
void tfToPose(tf::Transform &trans, geometry_msgs::PoseStamped &msg) { tf::quaternionTFToMsg(trans.getRotation(), msg.pose.orientation); msg.pose.position.x = trans.getOrigin().x(); msg.pose.position.y = trans.getOrigin().y(); msg.pose.position.z = trans.getOrigin().z(); }
bool computeMatrix(PointCloud::Ptr target, PointCloud::Ptr world, std::string target_name, std::string world_name, const bool broadcast) { if ((!world_name.empty()) && (!target_name.empty()) && (target->points.size() > 2) && (world->points.size() == target->points.size())) { Eigen::Matrix4f trMatrix; pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> svd; svd.estimateRigidTransformation(*target, *world, trMatrix); ROS_INFO("Registration completed and Registration Matrix is being broadcasted"); transform=tf::Transform(tf::Matrix3x3(trMatrix(0, 0), trMatrix(0, 1), trMatrix(0, 2), trMatrix(1, 0), trMatrix(1, 1), trMatrix(1, 2), trMatrix(2, 0), trMatrix(2, 1), trMatrix(2, 2)), tf::Vector3(trMatrix(0, 3), trMatrix(1, 3), trMatrix(2, 3))); Eigen::Vector3d origin(transform.getOrigin()); double roll, pitch, yaw; tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); std::cout << std::endl << "#################################################" << std::endl; std::cout << std::endl << "########### TRANSFORMATION PARAMETERS ###########" << std::endl; std::cout << std::endl << "#################################################" << std::endl; std::cout << "origin: "<<origin.transpose() << std::endl; std::cout << "rpy: " << roll << " " << pitch << " " << yaw << std::endl; } return true; }
/** * Send transform to FCU position controller * * Note: send only XYZ, Yaw */ void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) { // ENU frame tf::Vector3 origin = transform.getOrigin(); tf::Quaternion q = transform.getRotation(); /* Documentation start from bit 1 instead 0, * Ignore velocity and accel vectors, yaw rate */ uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3); if (uas->is_px4()) { /** * Current PX4 has bug: it cuts throttle if there no velocity sp * Issue #273. * * @todo Revesit this quirk later. Should be fixed in firmware. */ ignore_all_except_xyz_y = (1 << 11) | (7 << 6); } // ENU->NED. Issue #49. set_position_target_local_ned(stamp.toNSec() / 1000000, MAV_FRAME_LOCAL_NED, ignore_all_except_xyz_y, origin.y(), origin.x(), -origin.z(), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, tf::getYaw(q), 0.0); }
Affine3d eigen_from_tf(tf::Transform transform) { Affine3d x; x = Translation3d(vec2vec(transform.getOrigin())); tf::Quaternion q = transform.getRotation(); x *= Quaterniond(q.w(), q.x(), q.y(), q.z()); return x; }
// Move the end effector (tf jaco_tool_position) to a certain position void JacoCustom::moveToPoint(tf::Transform tf_){ geometry_msgs::Twist arm; arm.linear.x = tf_.getOrigin().getX(); arm.linear.y = tf_.getOrigin().getY(); arm.linear.z = tf_.getOrigin().getZ(); // double roll, pitch, yaw; // tf_.getBasis().getRPY(roll,pitch, yaw); // arm.angular.x = roll; // arm.angular.y = pitch; // arm.angular.z = yaw; wpi_jaco_msgs::QuaternionToEuler conv; geometry_msgs::Quaternion quat; tf::quaternionTFToMsg(tf_.getRotation(), quat); conv.request.orientation = quat; if(quaternion_to_euler_service_client.call(conv)){ arm.angular.x = conv.response.roll; arm.angular.y = conv.response.pitch; arm.angular.z = conv.response.yaw; } moveToPoint(arm); }
void PSMNode::publishOdom(const tf::Transform& transform, const ros::Time& time, const float cxx, const float cxy, const float cyy, const float ctt, const float dx, const float dy, const float dt) { nav_msgs::Odometry odom; tf::TransformBroadcaster odom_broadcaster; tf::Matrix3x3 m(transform.getRotation()); double roll, pitch, yaw, x, y; m.getRPY(roll, pitch, yaw); x = transform.getOrigin().getX(); y = transform.getOrigin().getY(); geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw); // there's almost certianly a better way to do this. // we're just republishing the existing transform as odom_topic_ /*geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = time; odom_trans.header.frame_id = odomTopic_; odom_trans.child_frame_id = "world"; odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0; odom_trans.transform.rotation = odom_quat; */ //odom_broadcaster.sendTransform(odom_trans); odom.header.stamp = time; odom.header.frame_id = odomTopic_; odom.child_frame_id = "world"; odom.pose.pose.position.x = x; odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; double dtime = (time - last_time).toSec(); odom.twist.twist.linear.x = dx / dtime; odom.twist.twist.linear.y = dy / dtime; odom.twist.twist.angular.z = dt / dtime; //cout << cxx << " " << cxy << " " << cyy << endl; odom.pose.covariance[0] = cxx / (ROS_TO_PM * ROS_TO_PM); odom.pose.covariance[1] = cxy / (ROS_TO_PM * ROS_TO_PM); odom.pose.covariance[6] = cxy / (ROS_TO_PM * ROS_TO_PM); odom.pose.covariance[7] = cyy / (ROS_TO_PM * ROS_TO_PM); odom.pose.covariance[35] = ctt; // cheat odom.pose.covariance[14] = 1e-6; odom.pose.covariance[21] = 99999999; odom.pose.covariance[28] = 99999999; odomPublisher_.publish(odom); }
static carmen_orientation_3D_t get_carmen_orientation_from_tf_transform(tf::Transform transform) { carmen_orientation_3D_t orientation; tf::Matrix3x3(transform.getRotation()).getEulerYPR(orientation.yaw, orientation.pitch, orientation.roll); return orientation; }
void planning_models::KinematicState::printTransform(const std::string &st, const tf::Transform &t, std::ostream &out) const { out << st << std::endl; const tf::Vector3 &v = t.getOrigin(); out << " origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl; const tf::Quaternion &q = t.getRotation(); out << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; }
geometry_msgs::Pose transformToPose(tf::Transform &trans) { geometry_msgs::Pose pose; pose.position.x = trans.getOrigin().x(); pose.position.y = trans.getOrigin().y(); pose.position.z = trans.getOrigin().z(); tf::quaternionTFToMsg(trans.getRotation(), pose.orientation); return pose; }
void PSMNode::tfToPose2D(const tf::Transform& t, geometry_msgs::Pose2D& pose) { tf::Matrix3x3 m(t.getRotation()); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); pose.x = t.getOrigin().getX(); pose.y = t.getOrigin().getY(); pose.theta = yaw; }
bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d) { if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true; double x = d.getOrigin().getX(); double y = d.getOrigin().getY(); if (x*x + y*y > kf_dist_linear_sq_) return true; return false; }
Eigen::Matrix4f EigenFromTF(tf::Transform trans) { Eigen::Matrix4f out; //btQuaternion quat = trans.getRotation(); tf::Quaternion quat( trans.getRotation()[0], trans.getRotation()[1], trans.getRotation()[2], trans.getRotation()[3] ); //btVector3 origin = trans.getOrigin(); tf::Vector3 origin( trans.getOrigin()[0], trans.getOrigin()[1], trans.getOrigin()[2]); Eigen::Quaternionf quat_out(quat.w(), quat.x(), quat.y(), quat.z()); Eigen::Vector3f origin_out(origin.x(), origin.y(), origin.z()); out.topLeftCorner<3,3>() = quat_out.toRotationMatrix(); out.topRightCorner<3,1>() = origin_out; out(3,3) = 1; return out; }
int main(int argc, char** argv) { ros::init(argc, argv, "displacement"); ros::NodeHandle node; ros::Subscriber sub1 = node.subscribe( "turtle1/pose", 10, &poseCallback1 ); ros::Subscriber sub2 = node.subscribe( "turtle2/pose", 10, &poseCallback2 ); ros::Publisher pub = node.advertise<geometry_msgs::Transform>( "displacement/transform", 1000 ); //Set the loop at 10 HZ ros::Rate rate(10); while (ros::ok()) { ros::spinOnce();//Call this function to process ROS incoming messages. //Calculate the transformation tf::Transform transformTmp; // transformTmp = transform1.inverse() * transform2; tf::Vector3 t1Origin = transform1.getOrigin(); tf::Vector3 t2Origin = transform2.getOrigin(); tf::Quaternion q1 = transform1.getRotation(); tf::Quaternion q2 = transform2.getRotation(); transformTmp.setOrigin( tf::Vector3(t2Origin.getX() - t1Origin.getX(), t2Origin.getY() - t1Origin.getY(), 0.0) ); tf::Quaternion qtemp; qtemp.setRPY(0, 0, q2.getAngle() - q1.getAngle()); transformTmp.setRotation(qtemp); //Publish the transformation geometry_msgs::Transform tg; tf::transformTFToMsg(transformTmp, tg); pub.publish(tg); rate.sleep();//Sleep the rest of the cycle until 10 Hz } return 0; }
std::vector<double> planning_models::KinematicModel::RevoluteJointModel::computeJointStateValues(const tf::Transform& transform) const { std::vector<double> ret; ROS_DEBUG_STREAM("Transform angle is " << transform.getRotation().getAngle() << " axis x " << transform.getRotation().getAxis().x() << " axis y " << transform.getRotation().getAxis().y() << " axis z " << transform.getRotation().getAxis().z()); double val = transform.getRotation().getAngle()*transform.getRotation().getAxis().dot(axis_); while(val < -M_PI) { if(val < -M_PI) { val += 2*M_PI; } } while(val > M_PI) { if(val > M_PI) { val -= 2*M_PI; } } ret.push_back(val); return ret; }
moveit_msgs::PlaceLocation tf_transform_to_place(tf::Transform t) { static int i = 0; moveit_msgs::PlaceLocation place; geometry_msgs::PoseStamped pose; tf::Vector3& origin = t.getOrigin(); tf::Quaternion rotation = t.getRotation(); tf::quaternionTFToMsg(rotation, pose.pose.orientation); pose.header.frame_id = "base_link"; pose.header.stamp = ros::Time::now(); pose.pose.position.x = origin.m_floats[0]; pose.pose.position.y = origin.m_floats[1]; pose.pose.position.z = origin.m_floats[2]; place.place_pose = pose; double x = place.place_pose.pose.position.x; double y = place.place_pose.pose.position.y; hypo(x,y,0.1); place.id = boost::lexical_cast<std::string>(i); //place.pre_place_approach.direction.vector.x = 1.0; place.pre_place_approach.direction.vector.z = -1.0; place.pre_place_approach.direction.header.frame_id = "katana_gripper_tool_frame"; place.pre_place_approach.min_distance = 0.07; place.pre_place_approach.desired_distance = 0.15; place.pre_place_approach.direction.header.stamp = ros::Time::now(); place.post_place_retreat.direction.vector.x = -1.0; //place.post_place_retreat.direction.vector.z = 1.0; place.post_place_retreat.min_distance = 0.07; place.post_place_retreat.desired_distance = 0.15; place.post_place_retreat.direction.header.frame_id = "katana_gripper_tool_frame"; place.post_place_retreat.direction.header.stamp = ros::Time::now(); // TODO: fill in grasp.post_place_retreat (copy of post_grasp_retreat?) place.post_place_posture.joint_names.push_back("katana_l_finger_joint"); place.post_place_posture.joint_names.push_back("katana_r_finger_joint"); place.post_place_posture.points.resize(1); place.post_place_posture.points[0].positions.push_back(0.3); place.post_place_posture.points[0].positions.push_back(0.3); place.allowed_touch_objects.resize(1); place.allowed_touch_objects[0] = "testbox"; i++; return place; }
void tfToXYZRPY( const tf::Transform& t, double& x, double& y, double& z, double& roll, double& pitch, double& yaw) { x = t.getOrigin().getX(); y = t.getOrigin().getY(); z = t.getOrigin().getZ(); tf::Matrix3x3 m(t.getRotation()); m.getRPY(roll, pitch, yaw); }
void fromTF(const tf::Transform &source, Eigen::Matrix4f &dest) { Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ()); q.normalize(); Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z()); Eigen::Matrix3f R(q.toRotationMatrix()); dest(0,0) = R(0,0); dest(0,1) = R(0,1); dest(0,2) = R(0,2); dest(1,0) = R(1,0); dest(1,1) = R(1,1); dest(1,2) = R(1,2); dest(2,0) = R(2,0); dest(2,1) = R(2,1); dest(2,2) = R(2,2); dest(3,0) = dest(3,1)= dest(3,2) = 0; dest(3,3) = 1; dest(0,3) = t(0); dest(1,3) = t(1); dest(2,3) = t(2); }
void MotionEstimation::constrainMotion(tf::Transform& motion) { // **** degree-of-freedom constraints if (motion_constraint_ == ROLL_PITCH) { tf::Quaternion q; q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation())); motion.setRotation(q); } else if (motion_constraint_ == ROLL_PITCH_Z) { tf::Quaternion q; q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation())); tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0); motion.setOrigin(p); motion.setRotation(q); } }
// calculates the linear and roatation speed out of given transform and duration and reverse drive information geometry_msgs::Twist CalibrateAction::calcTwistFromTransform(tf::Transform _transform, ros::Duration _dur) { geometry_msgs::Twist result_twist; result_twist.linear.x = (_transform.getOrigin().length() / _dur.toSec()) * ((_transform.getOrigin().getX() < 0) ? -1 : 1); result_twist.linear.y = 0; result_twist.linear.z = 0; result_twist.angular.x = 0; result_twist.angular.y = 0; result_twist.angular.z = (tf::getYaw(_transform.getRotation()) / _dur.toSec()); return result_twist; }
bool write_urdf() { //replace the urdf's camera pose with the calibrated pose double x,y,z,r,p,yaw; x=transform_gripper2camera.getOrigin().getX(); y=transform_gripper2camera.getOrigin().getY(); z=transform_gripper2camera.getOrigin().getZ(); tf::Matrix3x3(transform_gripper2camera.getRotation()).getRPY(r,p,yaw); std::stringstream updated_line; updated_line << " <origin xyz=\""; updated_line << x << " " << y << " " << z; updated_line << "\" rpy=\"" << r << " " << p << " " << yaw << "\" />"; ROS_INFO_STREAM("replacing old origin with: '" << updated_line.str() << "'"); lines.at(whichline)=updated_line.str(); //copy old file before writing in case something goes wrong ROS_INFO("Creating backup of urdf"); std::ifstream src(urdf_location.c_str(), std::ios::binary); std::stringstream temp; temp << urdf_location << ".temp"; std::ofstream dst(temp.str().c_str(),std::ios::binary); if (src.is_open() && dst.is_open()) dst << src.rdbuf(); else { ROS_ERROR("Error creating backup of urdf"); return false; } //write new urdf file with modified camera origin std::ofstream myfile; //open the file for writing, truncate because we're writing the whole thing myfile.open(urdf_location.c_str(), std::ios::out | std::ios::trunc); if (myfile.is_open()) { ROS_INFO("Urdf is open, writing..."); for (std::vector<std::string>::iterator it = lines.begin(); it != lines.end(); ++it) { //write each line to the file if (myfile.good()) myfile<<(*it)<<std::endl; else { ROS_ERROR("Something went wrong with writing the urdf"); return false; } } //end iterator myfile.close(); ROS_INFO("Success"); return true; } //end of 'if (myfile.is_open())' else { ROS_ERROR("Error opening the urdf file for writing"); return false; } }