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