本文整理汇总了C++中tf::Stamped::setData方法的典型用法代码示例。如果您正苦于以下问题:C++ Stamped::setData方法的具体用法?C++ Stamped::setData怎么用?C++ Stamped::setData使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf::Stamped
的用法示例。
在下文中一共展示了Stamped::setData方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: getRobotVel
void OdometryHelperRos::getRobotVel(tf::Stamped<tf::Pose>& robot_vel) {
// Set current velocities from odometry
geometry_msgs::Twist global_vel;
{
boost::mutex::scoped_lock lock(odom_mutex_);
global_vel.linear.x = base_odom_.twist.twist.linear.x;
global_vel.linear.y = base_odom_.twist.twist.linear.y;
global_vel.angular.z = base_odom_.twist.twist.angular.z;
robot_vel.frame_id_ = base_odom_.child_frame_id;
}
robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
robot_vel.stamp_ = ros::Time();
}
示例2: getGoalPose
bool getGoalPose (const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose)
{
if (global_plan.empty())
{
ROS_ERROR ("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try
{
tf::StampedTransform transform;
tf.waitForTransform (global_frame, ros::Time::now(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration (0.5));
tf.lookupTransform (global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF (plan_goal_pose, goal_pose);
goal_pose.setData (transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch (tf::LookupException& ex)
{
ROS_ERROR ("No Transform available Error: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR ("Connectivity Error: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR ("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR ("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int) global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}