Merge pull request autowarefoundation#607 from CPFL/hotfix/lane-select

Hotfix/lane select
tigerneil · Feb 27, 2017 · 4fbb032 · 4fbb032
2 parents 27a272c + 008d1d6
commit 4fbb032
Show file tree

Hide file tree

Showing 5 changed files with 42 additions and 18 deletions.
diff --git a/...puting/perception/localization/packages/autoware_connector/launch/vel_pose_connect.launch b/...puting/perception/localization/packages/autoware_connector/launch/vel_pose_connect.launch
@@ -7,14 +7,14 @@
   <arg name="sim_mode" default="false" />
 
   <group unless="$(arg sim_mode)">
-    <node pkg="autoware_connector" type="can_info_translator" name="can_info_translator" output="screen" />
-    <node pkg="topic_tools" type="relay" name="pose_relay" output="screen" args="$(arg topic_pose_stamped) /current_pose"/>
-    <node pkg="topic_tools" type="relay" name="vel_relay" output="screen" args="$(arg topic_twist_stamped) /current_velocity"/>
+    <node pkg="autoware_connector" type="can_info_translator" name="can_info_translator" output="log" />
+    <node pkg="topic_tools" type="relay" name="pose_relay" output="log" args="$(arg topic_pose_stamped) /current_pose"/>
+    <node pkg="topic_tools" type="relay" name="vel_relay" output="log" args="$(arg topic_twist_stamped) /current_velocity"/>
   </group>
 
   <group if="$(arg sim_mode)">
-    <node pkg="topic_tools" type="relay" name="pose_relay" output="screen" args="/sim_pose /current_pose"/>
-    <node pkg="topic_tools" type="relay" name="vel_relay" output="screen" args="/sim_velocity /current_velocity"/>
+    <node pkg="topic_tools" type="relay" name="pose_relay" output="log" args="/sim_pose /current_pose"/>
+    <node pkg="topic_tools" type="relay" name="vel_relay" output="log" args="/sim_velocity /current_velocity"/>
   </group>
 
 </launch>
diff --git a/ros/src/computing/planning/mission/packages/lane_planner/launch/lane_select.launch b/ros/src/computing/planning/mission/packages/lane_planner/launch/lane_select.launch
@@ -1,6 +1,6 @@
 <!-- -->
 <launch>
-	<node pkg="lane_planner" type="lane_select" name="lane_select" output="screen">
+	<node pkg="lane_planner" type="lane_select" name="lane_select" output="log">
 	</node>
 
 </launch>
diff --git a/...c/computing/planning/mission/packages/lane_planner/nodes/lane_select/lane_select_core.cpp b/...c/computing/planning/mission/packages/lane_planner/nodes/lane_select/lane_select_core.cpp
@@ -97,6 +97,7 @@ void LaneSelectNode::initForLaneSelect()
   // search closest waypoint number for each lanes
   if (!getClosestWaypointNumberForEachLanes())
   {
+    publishClosestWaypoint(-1);
     resetLaneIdx();
     return;
   }
@@ -105,9 +106,13 @@ void LaneSelectNode::initForLaneSelect()
   findNeighborLanes();
   updateChangeFlag();
   createLaneForChange();
-  publish(std::get<0>(tuple_vec_.at(current_lane_idx_)), std::get<1>(tuple_vec_.at(current_lane_idx_)),
-          std::get<2>(tuple_vec_.at(current_lane_idx_)));
+
+  publishLane(std::get<0>(tuple_vec_.at(current_lane_idx_)));
+  publishClosestWaypoint(std::get<1>(tuple_vec_.at(current_lane_idx_)));
+  publishChangeFlag(std::get<2>(tuple_vec_.at(current_lane_idx_)));
   publishVisualizer();
+
+  resetSubscriptionFlag();
   return;
 }
 
@@ -119,6 +124,13 @@ void LaneSelectNode::resetLaneIdx()
   publishVisualizer();
 }
 
+void LaneSelectNode::resetSubscriptionFlag()
+{
+  is_current_pose_subscribed_ = false;
+  is_current_velocity_subscribed_ = false;
+  is_current_state_subscribed_ = false;
+}
+
 void LaneSelectNode::processing()
 {
   if(!isAllTopicsSubscribed())
@@ -127,13 +139,15 @@ void LaneSelectNode::processing()
   // search closest waypoint number for each lanes
   if (!getClosestWaypointNumberForEachLanes())
   {
+    publishClosestWaypoint(-1);
     resetLaneIdx();
     return;
   }
 
   // if closest waypoint on current lane is -1,
   if (std::get<1>(tuple_vec_.at(current_lane_idx_)) == -1)
   {
+    publishClosestWaypoint(-1);
     resetLaneIdx();
     return;
   }
@@ -152,16 +166,21 @@ void LaneSelectNode::processing()
     std::get<2>(lane_for_change_) =
         static_cast<ChangeFlag>(std::get<0>(lane_for_change_).waypoints.at(std::get<1>(lane_for_change_)).change_flag);
     ROS_INFO("closest: %d", std::get<1>(lane_for_change_));
-    publish(std::get<0>(lane_for_change_), std::get<1>(lane_for_change_), std::get<2>(lane_for_change_));
+    publishLane(std::get<0>(lane_for_change_));
+    publishClosestWaypoint(std::get<1>(lane_for_change_));
+    publishChangeFlag(std::get<2>(lane_for_change_));
   }
   else
   {
     updateChangeFlag();
     createLaneForChange();
-    publish(std::get<0>(tuple_vec_.at(current_lane_idx_)), std::get<1>(tuple_vec_.at(current_lane_idx_)),
-            std::get<2>(tuple_vec_.at(current_lane_idx_)));
+
+    publishLane(std::get<0>(tuple_vec_.at(current_lane_idx_)));
+    publishClosestWaypoint(std::get<1>(tuple_vec_.at(current_lane_idx_)));
+    publishChangeFlag(std::get<2>(tuple_vec_.at(current_lane_idx_)));
   }
   publishVisualizer();
+  resetSubscriptionFlag();
 }
 
 int32_t LaneSelectNode::getClosestLaneChangeWaypointNumber(const std::vector<waypoint_follower::waypoint> &wps, int32_t cl_wp)
@@ -563,23 +582,25 @@ void LaneSelectNode::publishVisualizer()
   vis_pub1_.publish(marker_array);
 }
 
-void LaneSelectNode::publish(const waypoint_follower::lane &lane, const int32_t clst_wp, const ChangeFlag flag)
+void LaneSelectNode::publishLane(const waypoint_follower::lane &lane)
 {
   // publish global lane
   pub1_.publish(lane);
+}
 
+void LaneSelectNode::publishClosestWaypoint(const int32_t clst_wp)
+{
   // publish closest waypoint
   std_msgs::Int32 closest_waypoint;
   closest_waypoint.data = clst_wp;
   pub2_.publish(closest_waypoint);
+}
 
+void LaneSelectNode::publishChangeFlag(const ChangeFlag flag)
+{
   std_msgs::Int32 change_flag;
   change_flag.data = enumToInteger(flag);
   pub3_.publish(change_flag);
-
-  is_current_pose_subscribed_ = false;
-  is_current_velocity_subscribed_ = false;
-  is_current_state_subscribed_ = false;
 }
 
 void LaneSelectNode::callbackFromLaneArray(const waypoint_follower::LaneArrayConstPtr &msg)

diff --git a/...src/computing/planning/mission/packages/lane_planner/nodes/lane_select/lane_select_core.h b/...src/computing/planning/mission/packages/lane_planner/nodes/lane_select/lane_select_core.h
@@ -125,9 +125,12 @@ class LaneSelectNode
 
   // functions
   void resetLaneIdx();
+  void resetSubscriptionFlag();
   bool isAllTopicsSubscribed();
   void processing();
-  void publish(const waypoint_follower::lane &lane, const int32_t clst_wp, const ChangeFlag flag);
+  void publishLane(const waypoint_follower::lane &lane);
+  void publishClosestWaypoint(const int32_t clst_wp);
+  void publishChangeFlag(const ChangeFlag flag);
   bool getClosestWaypointNumberForEachLanes();
   int32_t findMostClosestLane(const std::vector<uint32_t> idx_vec, const geometry_msgs::Point p);
   void findCurrentLane();

diff --git a/ros/src/computing/planning/state/state_machine/launch/state_machine.launch b/ros/src/computing/planning/state/state_machine/launch/state_machine.launch
@@ -3,7 +3,7 @@
 
 	<arg name="is_manual_light_detection" default="True"/>
 	<!-- rosrun state_machine state_machine -->
-	<node pkg="state_machine" type="state_machine" name="state_machine" output="screen">
+	<node pkg="state_machine" type="state_machine" name="state_machine" output="log">
 		<param name="is_manual_light_detection" value="$(arg is_manual_light_detection)"/>
 	</node>