本文整理汇总了C++中Trajectory::getEndpoint方法的典型用法代码示例。如果您正苦于以下问题:C++ Trajectory::getEndpoint方法的具体用法?C++ Trajectory::getEndpoint怎么用?C++ Trajectory::getEndpoint使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Trajectory
的用法示例。
在下文中一共展示了Trajectory::getEndpoint方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: visualize_trajectory
void CalibrateAction::visualize_trajectory(Trajectory &traj)
{
// publishing to rviz
geometry_msgs::PoseArray poses;
for(unsigned int i=0; i<traj.getPointsSize();i++)
{
geometry_msgs::Pose temp_pose;
double x_, y_,th_;
traj.getPoint(i, x_, y_, th_);
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
temp_pose.position.x = x_;
temp_pose.position.y = y_;
poses.poses.push_back(temp_pose);
//ROS_INFO("Pose: x: %f y: %f th: %f", x_, y_, th_);
}
geometry_msgs::Pose temp_pose, temp_pose2;
double x_, y_,th_;
traj.getPoint(0, x_, y_, th_);
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
temp_pose.position.x = x_;
temp_pose.position.y = y_;
traj.getEndpoint(x_, y_, th_);
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose2.orientation);
temp_pose2.position.x = x_;
temp_pose2.position.y = y_;
//ROS_INFO("Visualize trajectory with %i points. First Point (%2.1f,%2.1f), last Point (%2.1f,%2.1f)", traj.getPointsSize(),temp_pose.position.x,temp_pose.position.y,temp_pose2.position.x,temp_pose2.position.y);
poses.header.frame_id = cost_map->getGlobalFrameID();;
estTraj_pub.publish(poses);
}
示例2: estimateFullPathIsClear
// estimates if, at current position, all space for speed up and continuing calibration run is free up to threshold, given the values in the parameters
bool CalibrateAction::estimateFullPathIsClear(double vx_samp, double vy_samp, double vtheta_samp, double sim_time_,
double acc_x, double acc_y, double acc_theta)
{
bool pathClear = false;
Trajectory tempTraj;
double x_, y_, theta_;
pathClear = checkPath(0, 0, 0, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, &tempTraj);
tempTraj.getEndpoint(x_, y_, theta_);
pathClear = checkPath(x_, y_, theta_, vx_samp, vy_samp, vtheta_samp, sim_time_, vx_samp, vy_samp, vtheta_samp, 0, 0, 0);
return pathClear;
}