Merge pull request #26 from CPFL/add-waypoint_clicker

Add waypoint_clicker

ros/src/computing/planning/motion/packages/waypoint_maker/CMakeLists.txt

@@ -42,6 +42,10 @@ target_link_libraries(waypoint_saver gnss ${catkin_LIBRARIES})
 add_dependencies(waypoint_saver
   waypoint_follower_generate_messages_cpp
   vehicle_socket_generate_messages_cpp)
+
+add_executable(waypoint_clicker nodes/waypoint_clicker/waypoint_clicker.cpp)
+target_link_libraries(waypoint_clicker ${catkin_LIBRARIES})
+
 add_executable(waypoint_marker_publisher nodes/waypoint_marker_publisher/waypoint_marker_publisher.cpp)
 target_link_libraries(waypoint_marker_publisher libwaypoint_follower ${catkin_LIBRARIES})
 add_dependencies(waypoint_marker_publisher

ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_clicker/waypoint_clicker.cpp

@@ -0,0 +1,90 @@
+/*
+ *  Copyright (c) 2015, Nagoya University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of Autoware nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <fstream>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+static tf::StampedTransform transform;
+static bool output_first;
+
+static int sub_point_queue_size;
+
+static double velocity;		// km/h
+static std::string output_file;
+
+static void write_clicked_point(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
+{
+	if (output_first) {
+		std::ofstream ofs(output_file.c_str());
+		ofs << std::fixed << msg->pose.pose.position.x + transform.getOrigin().x() << ","
+		    << std::fixed << msg->pose.pose.position.y + transform.getOrigin().y() << ","
+		    << std::fixed << msg->pose.pose.position.z + transform.getOrigin().z() << std::endl;
+	} else {
+		std::ofstream ofs(output_file.c_str(), std::ios_base::app);
+		ofs << std::fixed << msg->pose.pose.position.x + transform.getOrigin().x() << ","
+		    << std::fixed << msg->pose.pose.position.y + transform.getOrigin().y() << ","
+		    << std::fixed << msg->pose.pose.position.z + transform.getOrigin().z() << ","
+		    << std::fixed << velocity << std::endl;
+	}
+
+	if (output_first)
+		output_first = false;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "waypoint_clicker");
+
+	ros::NodeHandle n;
+	n.param<int>("/waypoint_clicker/sub_point_queue_size", sub_point_queue_size, 1);
+	n.param<double>("/waypoint_clicker/velocity", velocity, 40);
+	n.param<std::string>("/waypoint_clicker/output_file", output_file, "/tmp/lane_waypoint.csv");
+
+	ros::Subscriber sub_point = n.subscribe("/clicked_point", sub_point_queue_size, write_clicked_point);
+
+	tf::TransformListener listener;
+	try {
+		ros::Time zero = ros::Time(0);
+		listener.waitForTransform("map", "world", zero, ros::Duration(10));
+		listener.lookupTransform("map", "world", zero, transform);
+	} catch (tf::TransformException &ex) {
+		ROS_ERROR("%s", ex.what());
+	}
+	output_first = true;
+
+	ros::spin();
+
+	return 0;
+}

ros/src/util/packages/runtime_manager/scripts/computing.yaml

@@ -220,6 +220,9 @@ subs :
          gui  :
            save_filename :
              prop  : 1
+       - name : waypoint_clicker
+         cmd  : rosrun waypoint_maker waypoint_clicker
+         param: waypoint_clicker
     - name : waypoint_follower
       subs :   
        - name : pure_pursuit
@@ -854,6 +857,21 @@ params :
         dash        : ''
         delim       : ':='
 
+  - name  : waypoint_clicker
+    vars  :
+    - name      : velocity
+      label     : Velocity (km/h)
+      min       : 0
+      max       : 200
+      v         : 40.0
+      rosparam  : /waypoint_clicker/velocity
+    - name      : output_file
+      label     : Output File
+      kind      : path
+      path_type : save
+      v         : /tmp/lane_waypoint.csv
+      rosparam  : /waypoint_clicker/output_file
+
   - name  : waypoint_loader
     vars  :
     - name      : lane_waypoint