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);
}
// 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;
}