本文整理汇总了C++中tf::StampedTransform类的典型用法代码示例。如果您正苦于以下问题:C++ StampedTransform类的具体用法?C++ StampedTransform怎么用?C++ StampedTransform使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了StampedTransform类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: newPose
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;
}
开发者ID:lmc0709,项目名称:filteringprocess,代码行数:15,代码来源:utils.hpp
示例2: getNavFn
// 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);
}
开发者ID:embr,项目名称:cs225b,代码行数:16,代码来源:planner_simple.cpp
示例3: rosPointCloudToDataContainer
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;
}
开发者ID:mwd3,项目名称:hector_slam,代码行数:35,代码来源:HectorMappingRos.cpp
示例4: toString
/**
* 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();
}
开发者ID:andreasBihlmaier,项目名称:ahbros,代码行数:11,代码来源:ahbros.hpp
示例5: poseStampedToStampedTF
void poseStampedToStampedTF(geometry_msgs::PoseStamped &msg, tf::StampedTransform &stampedTF, std::string child)
{
tf::Transform btTrans;
stampedTF.stamp_ = msg.header.stamp;
stampedTF.frame_id_ = msg.header.frame_id;
stampedTF.child_frame_id_ = child;
tf::poseMsgToTF(msg.pose, btTrans);
stampedTF.setData(btTrans);
}
开发者ID:jiayil,项目名称:jiayi-ros-pkg,代码行数:10,代码来源:jlUtilities.cpp
示例6: makeViewFacingMarker
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);
}
开发者ID:alcor-lab,项目名称:trajectory-control,代码行数:43,代码来源:StigmergyPlanner.cpp
示例7: checkTF
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;
}
}
开发者ID:taroryu123,项目名称:waypoint_manager,代码行数:20,代码来源:rwrc15_waypoint_manager.cpp
示例8: lock
void sptam::sptam_node::fixOdomFrame(const CameraPose& cameraPose, const tf::StampedTransform& camera_to_odom, const ros::Time& t)
{
tf::Pose camera_to_map;
CameraPose2TFPose( cameraPose, camera_to_map );
// compute the new difference between map and odom
tf::Transform odom_to_map = camera_to_map * camera_to_odom.inverse();
std::lock_guard<std::mutex> lock( odom_to_map_mutex_ );
odom_to_map_ = odom_to_map;
}
开发者ID:Aerobota,项目名称:sptam,代码行数:11,代码来源:sptam_node.cpp
示例9: FINAL
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;
}
开发者ID:fmrico,项目名称:WaterMellon,代码行数:46,代码来源:WmObjectCapture.cpp
示例10: odomMsgToStampedTransformB
void InputOutputLinControl::odomMsgToStampedTransformB(double displacement, double yaw, nav_msgs::Odometry pose_odometry, tf::StampedTransform& pose_stamped)
{
pose_stamped.stamp_ = pose_odometry.header.stamp;
pose_stamped.frame_id_ = pose_odometry.header.frame_id;
pose_stamped.child_frame_id_ = pose_odometry.child_frame_id;
tf::Vector3 v;
v.setX(pose_odometry.pose.pose.position.x + displacement*cos(yaw));
v.setY(pose_odometry.pose.pose.position.y + displacement*sin(yaw));
v.setZ(pose_odometry.pose.pose.position.z);
pose_stamped.setOrigin(v);
tf::Quaternion q;
q.setX(pose_odometry.pose.pose.orientation.x);
q.setY(pose_odometry.pose.pose.orientation.y);
q.setZ(pose_odometry.pose.pose.orientation.z);
q.setW(pose_odometry.pose.pose.orientation.w);
pose_stamped.setRotation(q);
}
开发者ID:alcor-lab,项目名称:trajectory-control,代码行数:21,代码来源:InputOutputLinControl.cpp
示例11: foutAppendTF
void foutAppendTF(tf::StampedTransform &transform, std::ofstream &fout)
{
tf::Matrix3x3 rotMatrix = transform.getBasis();
tf::Vector3 originVector = transform.getOrigin();
tfScalar roll, pitch, yaw;
rotMatrix.getRPY(roll, pitch, yaw);
fout<< originVector.x() << " "
<< originVector.y() << " "
<< originVector.z() << " "
<< pcl::rad2deg(roll) << " "
<< pcl::rad2deg(pitch) << " "
<< pcl::rad2deg(yaw) << std::endl;
}
开发者ID:jiayil,项目名称:jiayi-ros-pkg,代码行数:22,代码来源:jlUtilities.cpp
示例12: updateRobotPoseByModel
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;
}
开发者ID:alcor-lab,项目名称:trajectory-control,代码行数:22,代码来源:InputOutputLinControl.cpp
示例13: printTF
void printTF(tf::StampedTransform &transform, std::string str)
{
tf::Matrix3x3 rotMatrix = transform.getBasis();
tf::Vector3 originVector = transform.getOrigin();
tfScalar roll, pitch, yaw;
rotMatrix.getRPY(roll, pitch, yaw);
ROS_INFO("TF %s (x y z r p y): %f %f %f %f %f %f",
str.c_str(),
originVector.x(),
originVector.y(),
originVector.z(),
pcl::rad2deg(roll),
pcl::rad2deg(pitch),
pcl::rad2deg(yaw));
}
开发者ID:jiayil,项目名称:jiayi-ros-pkg,代码行数:23,代码来源:jlUtilities.cpp
示例14: addRead
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);
}
开发者ID:Aharobot,项目名称:dukedusty2,代码行数:37,代码来源:HeatmapBuilder.cpp
示例15: get_pose_from_transform
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;
}
开发者ID:TuZZiX,项目名称:coke_fetcher,代码行数:24,代码来源:odom_tf_fncs.cpp
示例16: stf_inv
tf::StampedTransform OdomTf::stamped_transform_inverse(tf::StampedTransform stf) {
// instantiate stamped transform with constructor args
//note: child_frame and frame_id are reversed, to correspond to inverted transform
tf::StampedTransform stf_inv(stf.inverse(), stf.stamp_, stf.child_frame_id_, stf.frame_id_);
/* long-winded equivalent:
tf::StampedTransform stf_inv;
tf::Transform tf = get_tf_from_stamped_tf(stf);
tf::Transform tf_inv = tf.inverse();
stf_inv.stamp_ = stf.stamp_;
stf_inv.frame_id_ = stf.child_frame_id_;
stf_inv.child_frame_id_ = stf.frame_id_;
stf_inv.setOrigin(tf_inv.getOrigin());
stf_inv.setBasis(tf_inv.getBasis());
* */
return stf_inv;
}
开发者ID:TuZZiX,项目名称:coke_fetcher,代码行数:17,代码来源:odom_tf_fncs.cpp
示例17: printTF
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";
}
开发者ID:amazon-picking-challenge,项目名称:plocka_packa,代码行数:42,代码来源:print_state.cpp
示例18: catch
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 );
}
开发者ID:smd-cvlab-devel,项目名称:mocap_align,代码行数:37,代码来源:mocap_align.cpp
示例19: integratePoseMeasurement
void ObservationModel::integratePoseMeasurement(Particles& particles, double poseRoll, double posePitch, const tf::StampedTransform& footprintToTorso){
double poseHeight = footprintToTorso.getOrigin().getZ();
ROS_DEBUG("Pose measurement z=%f R=%f P=%f", poseHeight, poseRoll, posePitch);
// TODO cluster xy of particles => speedup
#pragma omp parallel for
for (unsigned i=0; i < particles.size(); ++i){
// integrate IMU meas.:
double roll, pitch, yaw;
particles[i].pose.getBasis().getRPY(roll, pitch, yaw);
particles[i].weight += m_weightRoll * logLikelihood(poseRoll - roll, m_sigmaRoll);
particles[i].weight += m_weightPitch * logLikelihood(posePitch - pitch, m_sigmaPitch);
// integrate height measurement (z)
double heightError = 0;
// if (getHeightError(particles[i],footprintToTorso, heightError))
// particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
}
}
开发者ID:caomw,项目名称:6d_localization,代码行数:22,代码来源:ObservationModel.cpp
示例20: getTrueRobotPose
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());
}
开发者ID:alcor-lab,项目名称:trajectory-control,代码行数:17,代码来源:InputOutputLinControl.cpp
注:本文中的tf::StampedTransform类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论