Merge pull request #16 from CPFL/publish-without-change-in-default-of…

…-vmap

Publish without change in default of vmap

ros/src/computing/planning/mission/packages/lane_planner/nodes/lane_navi/lane_navi.cpp

@@ -74,6 +74,11 @@ static void cache_left_lane()
 	}
 }
 
+static bool is_cached_vmap()
+{
+	return (!vmap_all.lanes.empty() && !vmap_all.nodes.empty() && !vmap_all.points.empty());
+}
+
 static void cache_lane(const map_file::LaneArray& msg)
 {
 	vmap_all.lanes = msg.lanes;
@@ -110,6 +115,11 @@ static void write_lane_waypoint(const geometry_msgs::Point& point, bool first)
 
 static void create_lane_waypoint(const tablet_socket::route_cmd& msg)
 {
+	if (!is_cached_vmap()) {
+		ROS_WARN("not cached vmap");
+		return;
+	}
+
 	geo_pos_conv geo;
 	geo.set_plane(7);
 	geo.llh_to_xyz(msg.point.front().lat, msg.point.front().lon, 0);
@@ -192,11 +202,6 @@ int main(int argc, char **argv)
 	ros::Subscriber sub_lane = n.subscribe("/vector_map_info/lane", sub_vmap_queue_size, cache_lane);
 	ros::Subscriber sub_node = n.subscribe("/vector_map_info/node", sub_vmap_queue_size, cache_node);
 	ros::Subscriber sub_point = n.subscribe("/vector_map_info/point_class", sub_vmap_queue_size, cache_point);
-	ros::Rate rate(1);
-	while (vmap_all.lanes.empty() || vmap_all.nodes.empty() || vmap_all.points.empty()) {
-		ros::spinOnce();
-		rate.sleep();
-	}
 	ros::Subscriber sub_route = n.subscribe("/route_cmd", sub_route_queue_size, create_lane_waypoint);
 
 	pub_waypoint = n.advertise<waypoint_follower::lane>("/lane_waypoint", pub_waypoint_queue_size,

ros/src/computing/planning/mission/packages/lane_planner/nodes/lane_rule/lane_rule.cpp

@@ -81,6 +81,12 @@ static void cache_left_lane()
 	}
 }
 
+static bool is_cached_vmap()
+{
+	return (!vmap_all.lanes.empty() && !vmap_all.nodes.empty() && !vmap_all.points.empty() &&
+		!vmap_all.stoplines.empty() && !vmap_all.dtlanes.empty());
+}
+
 static void cache_lane(const map_file::LaneArray& msg)
 {
 	vmap_all.lanes = msg.lanes;
@@ -442,11 +448,43 @@ static waypoint_follower::lane rule_stopline(const waypoint_follower::lane& msg,
 	return computations;
 }
 
+static void publish_waypoint_without_change(const waypoint_follower::lane& msg, const std_msgs::Header& header)
+{
+	waypoint_follower::lane traffic;
+	traffic.header = header;
+	traffic.increment = 1;
+	waypoint_follower::waypoint waypoint;
+	waypoint.pose.header = header;
+	waypoint.twist.header = header;
+
+	for (size_t i = 0; i < msg.waypoints.size(); ++i) {
+		waypoint.pose.pose = msg.waypoints[i].pose.pose;
+		waypoint.twist.twist = msg.waypoints[i].twist.twist;
+		traffic.waypoints.push_back(waypoint);
+	}
+
+	pub_traffic.publish(traffic);
+}
+
 static void create_traffic_waypoint(const waypoint_follower::lane& msg)
 {
 	std_msgs::Header header;
 	header.stamp = ros::Time::now();
 	header.frame_id = "/map";
+
+	if (!is_cached_vmap()) {
+		ROS_WARN("not cached vmap");
+		publish_waypoint_without_change(msg, header);
+		return;
+	}
+
+	std::vector<map_file::DTLane> dtlanes = search_dtlane(msg);
+	if (dtlanes.size() != msg.waypoints.size()) {
+		ROS_WARN("not found dtlane");
+		publish_waypoint_without_change(msg, header);
+		return;
+	}
+
 	waypoint_follower::lane green;
 	green.header = header;
 	green.increment = 1;
@@ -457,21 +495,6 @@ static void create_traffic_waypoint(const waypoint_follower::lane& msg)
 	waypoint.pose.header = header;
 	waypoint.twist.header = header;
 
-	std::vector<map_file::DTLane> dtlanes = search_dtlane(msg);
-	if (dtlanes.size() != msg.waypoints.size()) {
-		ROS_WARN("not found dtlane");
-
-		// publish waypoints without change
-		for (size_t i = 0; i < msg.waypoints.size(); ++i) {
-			waypoint.pose.pose = msg.waypoints[i].pose.pose;
-			waypoint.twist.twist = msg.waypoints[i].twist.twist;
-			green.waypoints.push_back(waypoint);
-		}
-		pub_traffic.publish(green);
-
-		return;
-	}
-
 	for (size_t i = 0; i < msg.waypoints.size(); ++i) {
 		double reduction = dtlane_to_reduction(dtlanes, i);
 
@@ -523,12 +546,6 @@ int main(int argc, char **argv)
 	ros::Subscriber sub_point = n.subscribe("/vector_map_info/point_class", sub_vmap_queue_size, cache_point);
 	ros::Subscriber sub_stopline = n.subscribe("/vector_map_info/stop_line", sub_vmap_queue_size, cache_stopline);
 	ros::Subscriber sub_dtlane = n.subscribe("/vector_map_info/dtlane", sub_vmap_queue_size, cache_dtlane);
-	ros::Rate rate(1);
-	while (vmap_all.lanes.empty() || vmap_all.nodes.empty() || vmap_all.points.empty() ||
-	       vmap_all.stoplines.empty() || vmap_all.dtlanes.empty()) {
-		ros::spinOnce();
-		rate.sleep();
-	}
 	ros::Subscriber sub_waypoint = n.subscribe("/lane_waypoint", sub_waypoint_queue_size, create_traffic_waypoint);
 	ros::Subscriber sub_config = n.subscribe("/config/lane_rule", sub_config_queue_size, config_rule);