27 #ifndef _trajectory_planner_nodelet_CPP_
28 #define _trajectory_planner_nodelet_CPP_
43 std::vector< pcl::PointCloud<pcl::PointXYZ> >
pc_v;
58 pose_in.pose.orientation.w=cos(msg.theta/2.0);
61 pose_in.pose.orientation.z=sin(msg.theta/2.0);
63 pose_in.header.frame_id=
"/world";
65 manage_vt->set_attractor_point(msg.x,msg.y,msg.theta);
77 ROS_INFO(
"received mtt num obs=%ld num lines first obs =%d frame_id=%s",msg.id.size(), msg.obstacle_lines[0].width, msg.header.frame_id.c_str());
81 for (
size_t i=0; i<msg.id.size(); ++i)
83 pcl::PointCloud<pcl::PointXYZ> tmp;
84 pcl::fromROSMsg(msg.obstacle_lines[i], tmp);
85 tmp.header.frame_id=msg.header.frame_id;
88 ROS_INFO(
"Obstacle %ld has %ld lines", i, tmp.points.size());
101 vector<double> speed_setted;
104 if(i < (_NUM_NODES_ - 1))
106 if((t->arc[i])*(t->arc[i+1])<0.0)
132 int main(
int argc,
char **argv)
134 ros::init(argc, argv,
"trajectory_planner_nodelet");
135 ros::NodeHandle n(
"~");
139 tf::TransformBroadcaster broadcaster;
140 tf::TransformListener listener;
142 ros::Publisher array_pub = n.advertise<visualization_msgs::MarkerArray>(
"/array_of_markers", 1 );
143 ros::Subscriber sub = n.subscribe(
"/msg_coordinates", 1,
set_coordinates);
144 ros::Subscriber mtt_sub = n.subscribe(
"/mtt_targets", 1,
mtt_callback);
145 ros::Rate loop_rate(10);
174 vector<double> v_arc;
177 v_a.push_back(M_PI/180.*
a[i][j]);
178 v_arc.push_back(
arc[i][j]);
180 manage_vt->create_new_trajectory(v_a,v_arc,v_a);
188 ROS_ERROR(
"Chosen trajectory does not exist");
195 commandPublisher = n.advertise<trajectory_planner::traj_info>(
"/trajectory_information", 1000);
209 ROS_INFO(
"Going to plan a trajectory");
214 bool have_transform=
true;
220 catch (tf::TransformException ex)
222 ROS_ERROR(
"%s",ex.what());
223 have_transform=
false;
228 ros::Time time=ros::Time::now();
229 broadcaster.sendTransform(tf::StampedTransform(
transformw, time,
"/world",
"/parking_frame"));
231 broadcaster.sendTransform(tf::StampedTransform(
transformw, time+ros::Duration(5),
"/world",
"/parking_frame"));
235 ros::Duration(0.1).sleep();
244 pose_in.header.stamp=time+ros::Duration(0.1);
247 ROS_INFO(
"pose_in frame_id=%s pose_transformed frame_id=%s",
pose_in.header.frame_id.c_str(),
pose_transformed.header.frame_id.c_str());
253 pcl::PointCloud<pcl::PointXYZ> pct;
254 mtt::TargetListPC msg_transformed;
258 for (
size_t i=0; i<
pc_v.size(); ++i)
267 catch (tf::TransformException ex)
269 ROS_ERROR(
"%s",ex.what());
273 ROS_INFO(
"pc_v[%ld] frame_id=%s pcp frame_id=%s",i,
pc_v[i].header.frame_id.c_str(), pct.header.frame_id.c_str());
276 sensor_msgs::PointCloud2 pc_msg;
277 pcl::toROSMsg(pct, pc_msg);
278 pc_msg.header.frame_id =
"/parking_frame";
279 pc_msg.header.stamp = ros::Time::now();
280 msg_transformed.obstacle_lines.push_back(pc_msg);
282 manage_vt->set_obstacles(msg_transformed);
293 ros::Time st=ros::Time::now();
295 manage_vt->compute_trajectories_scores();
297 cout<<
"Compute scores: "<<(ros::Time::now()-st).toSec();
299 ROS_INFO(
"manage_vt chosen traj= %d",
manage_vt->chosen_traj.index);
302 trajectory_planner::traj_info
info;
304 manage_vt->get_traj_info_msg_from_chosen(&info);
309 visualization_msgs::MarkerArray marker_array;
310 manage_vt->compute_vis_marker_array(&marker_array);
311 array_pub.publish(marker_array);
312 cout<<
"ja size:"<<marker_array.markers.size()<<endl;
324 broadcaster.sendTransform(tf::StampedTransform(
transformw, ros::Time::now(),
"/world",
"/parking_frame"));
325 cout<<
"stat Publishing transform"<<endl;
int main(int argc, char **argv)
Main code of the nodelet.
vector< double > set_speed_vector(boost::shared_ptr< c_trajectory > t)
Determine the speed vector.
#define _VEHICLE_LENGHT_FRONT_
ros::Publisher commandPublisher
std::vector< pcl::PointCloud< pcl::PointXYZ > > pc_v
_EXTERN_ c_manage_trajectoryPtr manage_vt
_EXTERN_ tf::TransformListener * p_listener
_EXTERN_ tf::StampedTransform transformw
#define _VEHICLE_LENGHT_BACK_
trajectory_planner::traj_info info
boost::shared_ptr< c_manage_trajectory > c_manage_trajectoryPtr
_EXTERN_ tf::StampedTransform transform_mtt
void mtt_callback(mtt::TargetListPC msg)
Set mtt points.
void set_coordinates(trajectory_planner::coordinates msg)
Set attractor point coordinates.
double a[20][9]
Trajectories definitions.
geometry_msgs::PoseStamped pose_in
geometry_msgs::PoseStamped pose_transformed
#define _VEHICLE_HEIGHT_TOP_
#define _VEHICLE_HEIGHT_BOTTOM_