Merge branch 'master' of https://github.com/CPFL/Autoware

autowarefoundation · Sep 14, 2015 · 7589b36 · 7589b36
2 parents a52983e + dbaacce
commit 7589b36
Show file tree

Hide file tree

Showing 22 changed files with 3,717 additions and 30 deletions.
diff --git a/ros/src/.config/quick_start/sample_lidar_camera/mission_planning.launch b/ros/src/.config/quick_start/sample_lidar_camera/mission_planning.launch
@@ -18,9 +18,9 @@
   <node pkg="lane_planner" type="lane_rule" name="lane_rule" />
 
   <!-- lane_change -->
-  <node pkg="lane_planner" type="lane_change" name="lane_change" />
+  <node pkg="lane_planner" type="lane_select" name="lane_select" />
 
   <!-- velocity_set -->
-  <node pkg="driving_planner" type="velocity_set" name="velocity_set" />
+  <node pkg="waypoint_follower" type="velocity_set" name="velocity_set" />
 
 </launch>
diff --git a/...puting/perception/localization/packages/ndt_localizer/nodes/ndt_matching/ndt_matching.cpp b/...puting/perception/localization/packages/ndt_localizer/nodes/ndt_matching/ndt_matching.cpp
@@ -139,8 +139,8 @@ static std_msgs::Bool ndt_stat_msg;
 static ros::Time current_scan_time;
 static ros::Time previous_scan_time;
 static ros::Duration scan_duration;
-static ros::Publisher estimated_vel_pub;
-static std_msgs::Float32 estimated_vel;
+static ros::Publisher estimated_vel_mps_pub, estimated_vel_kmph_pub;
+static std_msgs::Float32 estimated_vel_mps, estimated_vel_kmph;
 
 static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;;
 static ros::Publisher time_ndt_matching_pub;
@@ -855,9 +855,11 @@ static void velodyne_callback(const pcl::PointCloud<velodyne_pointcloud::PointXY
       scan_duration = current_scan_time - previous_scan_time;
       double secs = scan_duration.toSec();
       double distance = sqrt((current_pos.x - previous_pos.x) * (current_pos.x - previous_pos.x) + (current_pos.y - previous_pos.y) * (current_pos.y - previous_pos.y) + (current_pos.z - previous_pos.z) * (current_pos.z - previous_pos.z));
-      estimated_vel.data = distance * 3.6 / secs;
+      estimated_vel_mps.data = distance / secs;
+      estimated_vel_kmph.data = distance * 3.6 / secs;
 
-      estimated_vel_pub.publish(estimated_vel);
+      estimated_vel_mps_pub.publish(estimated_vel_mps);
+      estimated_vel_kmph_pub.publish(estimated_vel_kmph);
 
       // Output csv
       std::ofstream ofs("matching_time.csv", std::ios::app);
@@ -973,7 +975,9 @@ int main(int argc, char **argv)
 
     ndt_stat_pub = nh.advertise<std_msgs::Bool>("/ndt_stat", 1000);
 
-    estimated_vel_pub = nh.advertise<std_msgs::Float32>("/estimated_vel", 1000);
+    estimated_vel_mps_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_mps", 1000);
+
+    estimated_vel_kmph_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_kmph", 1000);
 
     time_ndt_matching_pub = nh.advertise<std_msgs::Float32>("/time_ndt_matching", 1000);
 

diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt
@@ -101,7 +101,7 @@ add_executable(twist_through nodes/collision_avoid/twist_through.cpp)
 target_link_libraries(twist_through ${catkin_LIBRARIES})
 
 add_executable(velocity_set nodes/velocity_set/velocity_set.cpp)
-target_link_libraries(velocity_set ${catkin_LIBRARIES})
-add_dependencies(velocity_set
-  waypoint_follower_generate_messages_cpp)
+target_link_libraries(velocity_set libwaypoint_follower ${catkin_LIBRARIES})
+add_dependencies(velocity_set 
+waypoint_follower_generate_messages_cpp)
 
diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/launch/velocity_set.launch b/ros/src/computing/planning/motion/packages/waypoint_follower/launch/velocity_set.launch
@@ -0,0 +1,21 @@
+<!-- -->
+<launch>
+
+	<arg name="detection_range" default="1.3" /> <!-- meter 2.2 -->
+	<arg name="threshold_points" default="15" /> <!-- 5 -->
+	<arg name="stop_interval" default="15" /> 
+	<arg name="detection_height_top" default="2.0" /><!-- 2.0 -->
+	<arg name="detection_height_bottom" default="-1.0" />
+    <arg name="current_pose_topic" default="ndt" /> 
+
+	<!-- rosrun waypoint_follower velocity_set _detection_range:=0 -->
+	<node pkg="waypoint_follower" type="velocity_set" name="velocity_set" output="screen">
+		<param name="detection_range" value="$(arg detection_range)" />
+		<param name="threshold_points" value="$(arg threshold_points)" />
+		<param name="stop_interval" value="$(arg stop_interval)" />
+		<param name="detection_height_top" value="$(arg detection_height_top)" />
+		<param name="detection_height_bottom" value="$(arg detection_height_bottom)" />
+		<param name="current_pose_topic" value="$(arg current_pose_topic)" />
+	</node>
+
+</launch>