void Trajectory_Tracker::track_trajectory(const planner::gen_trajGoalConstPtr &goal)
{
ROS_INFO("TRACKING TRAJECTORY");
float x = goal->x;
float y = goal->y;
x = x;
y = y;
if(goal->frame != "robot"){
geometry_msgs::Point goal_point = transform_goal_to_robot(x,y,goal->frame);
x = goal_point.x;
y = goal_point.y;
}
bool following_trajectory = true;
tf::StampedTransform frame_transform = broadcast_new_traj_frame();
current_trajectory.generate_trajectory(x,y);
cout << "generated trajectory" << endl;
ros::Rate loop_rate(10);
tf::StampedTransform transform;
tf::Quaternion q;
ros::Time start_time = ros::Time::now();
float t = 0;
while(t<=(current_trajectory.t_end)+1)
{
if (action_server.isPreemptRequested() || !ros::ok())
{
float u_v = 0;
float u_w = 0;
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = u_v;
cmd_vector.y = 0;
cmd_vector.theta = u_w;
cmd_pub.publish(cmd_vector);
result.success = 0;
action_server.setSucceeded(result);
cout << "CANCELING GOAL" << endl;
return;
}
t = (ros::Time::now()-start_time).toSec();
feedback.time = t;
action_server.publishFeedback(feedback);
Eigen::ArrayXf traj_at_t = current_trajectory.traj_time_lookup(t);
marker_pub.publish(current_trajectory.points);
marker_pub.publish(current_trajectory.line_strip);
// marker_pub.publish(current_trajectory.line_list);
transform.setOrigin( tf::Vector3(traj_at_t(1), traj_at_t(2), 0));
q.setRPY(0, 0, traj_at_t(3));
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/traj_frame", "/target"));
Eigen::ArrayXf error = get_error();
cout << error << endl;
// TODO: FEEDFORWARD
float u_v = traj_at_t(4);
float u_w = traj_at_t(5);
u_v += Kp_x*error(0);
u_w += Kp_y*error(1) + Kp_w*error(2);
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = u_v;
cmd_vector.y = 0;
cmd_vector.theta = u_w;
cmd_pub.publish(cmd_vector);
loop_rate.sleep();
if(!ros::ok())
{
result.success = 0;
action_server.setSucceeded(result);
return;
}
br.sendTransform(tf::StampedTransform(frame_transform, ros::Time::now(), "/world", "/traj_frame"));
}
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = 0;
cmd_vector.y = 0;
cmd_vector.theta = 0;
cmd_pub.publish(cmd_vector);
result.success = 1;
action_server.setSucceeded(result);
}
