Waypoint selector recorder

applications/eklavya3/eklavya3_roboteq/launch/module.launch

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-	<node pkg="eklavya_roboteq" type="server" name="server" respawn="true" output="screen"/>
 	<node pkg="eklavya_roboteq" type="eklavya_controller_client" name="eklavya_controller_client" output="screen"/>
+	<node pkg="eklavya_roboteq" type="server" name="server" respawn="true" output="screen"/>
 </launch>

environment/interpretation/data_fuser/src/main.cpp

@@ -18,7 +18,7 @@ void exit_with_help() {
 }
 
 int main(int argc, char** argv) {
-    int first_subscriber_flag = 1, second_subscriber_flag = 1, debug_flag = 0;
+    int first_subscriber_flag = 1, second_subscriber_flag = 0, debug_flag = 0;
 
     std::string first_subscriber_topic_name, second_subcriber_topic_name, node_id, node_name, publisher_topic_name;
     first_subscriber_topic_name = std::string("/obstacle_detector/obstacles");
@@ -70,7 +70,7 @@ int main(int argc, char** argv) {
         }
     }
 
-    publisher_topic_name = std::string("/map");
+    publisher_topic_name = std::string("/data_fuser/map");
     node_name = node_name + node_id;
 
     ros::init(argc, argv, node_name.c_str());

environment/interpretation/lane_detector/launch/module.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <launch>
-    <node pkg="lane_detector" type="laneDetectorNode" name="lane_detector" args="6" output="screen" cwd="node">
+    <node pkg="lane_detector" type="lane_detector" name="lane_detector" args="6" output="screen" cwd="node">
         <param name="debug_mode" type="int" value="0"/>
         <param name="ipt_offsets_file" type="str" value="../data/ipt_offsets0.txt"/>                
         <param name="map_size" type="int" value="1000"/>        

environment/interpretation/obstacle_detector/src/ObstacleDetector.cpp

@@ -70,7 +70,7 @@ ObstacleDetector::ObstacleDetector(std::string node_name, ros::NodeHandle& node_
     obstacle_map = cv::Mat(map_size, map_size, CV_8UC1, cvScalarAll(0));
     image_transport = new image_transport::ImageTransport(node_handle);
     obstacle_publisher = image_transport->advertise(std::string("/") + node_name + std::string("/obstacles"), 10);
-    scan_subscriber = node_handle.subscribe("/scan", 2, &ObstacleDetector::scanCallback, this);
+    scan_subscriber = node_handle.subscribe("/sensors/hokuyo_nodes/0", 2, &ObstacleDetector::scanCallback, this);
 }
 
 ObstacleDetector::~ObstacleDetector() {

environment/interpretation/waypoint_selector/CMakeLists.txt

@@ -30,4 +30,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #target_link_libraries(example ${PROJECT_NAME})
 rosbuild_add_executable(waypoint_selector 
                         src/main.cpp
-                        src/waypoint_selector.cpp)
+                        src/waypoint_selector.cpp)
+rosbuild_add_executable(waypoint_recorder
+                        src/waypoint_recorder.cpp)

environment/interpretation/waypoint_selector/launch/recorder.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+
+<launch>
+    <node pkg="waypoint_selector" type="waypoint_recorder" name="waypoint_recorder" args="" output="screen" cwd="node">
+        <param name="data_file" type="str" value="../data/.count.dat"/>
+        <param name="record_file_folder" type="str" value="../data/"/>                      
+    </node>
+</launch>

environment/interpretation/waypoint_selector/src/waypoint_recorder.cpp

@@ -0,0 +1,104 @@
+#include <vector>
+#include <sensor_msgs/NavSatFix.h>
+#include <ros/ros.h>
+#include <boost/bind.hpp>
+#include <string>
+#include <cmath>
+#include <iostream>
+#include <ios>
+#include <iosfwd>
+#include <fstream>
+#include <iomanip>
+
+static const int buffer_size = 10;
+static const int loop_rate_hz = 10;
+
+sensor_msgs::NavSatFix current_gps;
+
+void setCurrentGPS(sensor_msgs::NavSatFix sensor_gps) {
+    current_gps = sensor_gps;
+}
+
+int main(int argc, char* argv[]) {
+    int file_count, i, no_of_waypoints, count;
+    std::string c;
+    sensor_msgs::NavSatFix term, temp;
+    std::string num_file, txtfile_add, file_num_str;
+    std::fstream numfile, waypoints_file, edit_numfile;
+    std::vector<sensor_msgs::NavSatFix> waypoints_vector;
+    sensor_msgs::NavSatFix temp_pair;
+
+    ros::init(argc, argv, "waypoint_recorder");
+    ros::NodeHandle node_handle;
+
+    node_handle.getParam("/waypoint_recorder/data_file", num_file);
+    node_handle.getParam("/waypoint_recorder/record_file_folder", txtfile_add);
+
+    numfile.open(num_file.c_str());
+
+    if (numfile.is_open()) {
+        numfile>>file_count;
+        std::stringstream ss;
+        ss << file_count;
+        file_num_str = ss.str();
+        numfile.close();
+    } else {
+        file_count = 0;
+        numfile.close();
+    }
+    std::cout << "Enter the no of waypoints:";
+    std::cin>>no_of_waypoints;
+    std::cin.ignore(1, '\n');
+
+    ros::Subscriber sub_current_gps = node_handle.subscribe("/vn_ins/fix", buffer_size, setCurrentGPS);
+
+    ros::Rate loop_rate(loop_rate_hz);
+    count = 0;
+    while (ros::ok() && count < no_of_waypoints) {
+        ros::spinOnce();
+        for (i = 0, term.latitude = current_gps.latitude, term.longitude = current_gps.longitude; term.latitude != 0 && term.longitude != 0; ++i) {
+            std::cout << "ENTER to record an entry\n'e' to end\nand 'n'(only after 'e') to close file when finished taking waypoints and make a txt." << std::endl;
+            std::getline(std::cin, c);
+            if (c == "") {
+                std::cout << "Reading " << count + 1 << "th waypoint " << i + 1 << "th time." << std::endl;
+                ros::spinOnce();
+                temp = current_gps;
+                std::cout << "latitude:" << std::setprecision(10) << temp.latitude << " " << "longitude:" << std::setprecision(10) << temp.longitude << std::endl;
+                term.latitude = (((term.latitude * i) + temp.latitude) / (i + 1));
+                term.longitude = (((term.longitude * i) + temp.longitude) / (i + 1));
+                temp_pair.latitude = term.latitude;
+                temp_pair.longitude = term.longitude;
+                continue;
+            } else if (c == "e") {
+                std::cout << "\n";
+                break;
+            } else if (c == "n") {
+                std::cout << "\n";
+                break;
+            }
+        }
+        if (c == "e") {
+            waypoints_vector.push_back(temp_pair);
+            count++;
+            std::cout << "Read " << count << "th waypoint\n Move to the next waypoint.\n" << std::endl;
+            std::cout << "------------------------------------" << std::endl;
+        } else if (c == "n") {
+            break;
+        }
+    }
+    edit_numfile.open(num_file.c_str(), std::ios::in | std::ios::out | std::ios::trunc);
+    edit_numfile << ++file_count;
+    edit_numfile.close();
+
+    std::string record_file = (txtfile_add + "GPS_waypoints" + file_num_str + ".txt");
+    std::cout << "Writing to " << record_file << std::endl;
+    waypoints_file.open(record_file.c_str(), std::ios::in | std::ios::out | std::ios::trunc);
+    waypoints_file << count << std::endl;
+    for (std::vector <sensor_msgs::NavSatFix>::iterator it = waypoints_vector.begin(); it != waypoints_vector.end(); it++) {
+        waypoints_file << std::setprecision(10) << it->latitude;
+        waypoints_file << " ";
+        waypoints_file << std::setprecision(10) << it->longitude << std::endl;
+    }
+    waypoints_file << "#mention this file in the launch of waypoint_selector." << std::endl;
+    waypoints_file.close();
+}

environment/sensing/vn_ins/src/vn_ins.cpp

@@ -16,13 +16,13 @@ vectorNav::~vectorNav() {
 }
 
 bool vectorNav::connect() {
-    //vn200_connect(&vn200, vn200_com_port.c_str(), baud_rate);
+    vn200_connect(&vn200, vn200_com_port.c_str(), baud_rate);
     vn100_connect(&vn100, vn100_com_port.c_str(), baud_rate);
     return true;
 }
 
 bool vectorNav::disconnect() {
-    //vn200_disconnect(&vn200);
+    vn200_disconnect(&vn200);
     vn100_disconnect(&vn100);
     return true;
 }
@@ -40,30 +40,30 @@ bool vectorNav::fetch() {
     double gpsTime, latitude, longitude, altitude;
     VnVector3 magnetic, acceleration, angularRate, ypr, latitudeLognitudeAltitude, nedVelocity, positionAccuracy;
 
-    //vn200_getGpsSolution(&vn200, &gpsTime, &gpsWeek, &gpsFix, &numberOfSatellites, &latitudeLognitudeAltitude, &nedVelocity, &positionAccuracy, &speedAccuracy, &timeAccuracy);
+    vn200_getGpsSolution(&vn200, &gpsTime, &gpsWeek, &gpsFix, &numberOfSatellites, &latitudeLognitudeAltitude, &nedVelocity, &positionAccuracy, &speedAccuracy, &timeAccuracy);
     ROS_INFO("Triangulating from %d satellites", numberOfSatellites);
 
-    /*_gps.latitude = latitudeLognitudeAltitude.c0;
+    _gps.latitude = latitudeLognitudeAltitude.c0;
     _gps.longitude = latitudeLognitudeAltitude.c1;
-    _gps.altitude = latitudeLognitudeAltitude.c2;*/
+    _gps.altitude = latitudeLognitudeAltitude.c2;
 
     _yaw.data = yaw;
 
     return true;
 }
 
 void vectorNav::publish(int frame_id) {
-    //fix_publisher.publish(_gps);
+    fix_publisher.publish(_gps);
     yaw_publisher.publish(_yaw);
 }
 
 void vectorNav::initializeParameters() {
     baud_rate = 115200;
     message_queue_size = 10;
     node_name = std::string("vn_ins");
-    //fix_topic_name = node_name + std::string("/fix");
+    fix_topic_name = node_name + std::string("/fix");
     yaw_topic_name = node_name + std::string("/yaw");
-    //vn200_com_port = std::string("/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTUTUVO5-if00-port0");
+    vn200_com_port = std::string("/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTUTUVO5-if00-port0");
     vn100_com_port = std::string("/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTVJUC0O-if00-port0");
 }
 
@@ -73,12 +73,12 @@ void vectorNav::initializeParameters(int argc, char** argv) {
     node_name = std::string("vn_ins") + std::string(argv[1]);
     fix_topic_name = node_name + std::string(argv[1]) + std::string("/fix");
     yaw_topic_name = node_name + std::string(argv[1]) + std::string("/yaw");
-    //vn200_com_port = std::string(argv[2]);
+    vn200_com_port = std::string(argv[2]);
     vn100_com_port = std::string(argv[3]);
 }
 
 void vectorNav::setupCommunications() {
-    //fix_publisher = node_handle->advertise<sensor_msgs::NavSatFix>(fix_topic_name.c_str(), message_queue_size);
+    fix_publisher = node_handle->advertise<sensor_msgs::NavSatFix>(fix_topic_name.c_str(), message_queue_size);
     yaw_publisher = node_handle->advertise<std_msgs::Float64>(yaw_topic_name.c_str(), message_queue_size);
 }
 

planning/motion_planning/local_planner/launch/module.launch

@@ -5,7 +5,7 @@
         <param name="distance_transform" type="int" value="1"/>
         <param name="seed_file2" type="str" value="/seeds/seeds2.txt"/>
         <param name="seed_file8" type="str" value="/seeds/seeds8.txt"/>
-        <param name="planning_strategy" type="int" value="1"/>
+        <param name="planning_strategy" type="int" value="0"/>
         <param name="map_max_rows" type="int" value="1000"/>
         <param name="map_max_cols" type="int" value="1000"/>
         <param name="loop_rate" type="int" value="10"/>
@@ -19,10 +19,5 @@
         <param name="max_iterations_load_given_seeds" type="int" value="10000"/>
 
     </node>
-    <node pkg="showpath_debugger" type="debugger" name="debugger" output="screen" cwd="node">
-        <param name="loop_rate" type="int" value="10"/>
-        <param name="map_max_rows" type="int" value="1000"/>
-        <param name="map_max_cols" type="int" value="1000"/>
-    </node>
 </launch>
 

planning/motion_planning/local_planner/src/local_planner.cpp

@@ -15,7 +15,7 @@ namespace navigation {
         node_handle.getParam("local_planner/map_max_rows", map_max_rows);
         node_handle.getParam("local_planner/map_max_cols", map_max_cols);
 
-        fusion_map_subscriber = node_handle.subscribe("data_fuser/map", 10, &LocalPlanner::updateFusionMap, this);
+        fusion_map_subscriber = node_handle.subscribe("/data_fuser/map", 10, &LocalPlanner::updateFusionMap, this);
         target_subscriber = node_handle.subscribe("strategy_planner/target", 10, &LocalPlanner::updateTargetPose, this);
         planning_strategy_subscriber = node_handle.subscribe("strategy_planner/which_planner", 10, &LocalPlanner::updateStrategy, this);
 

planning/motion_planning/showpath_debugger/src/debugger.cpp

@@ -11,10 +11,12 @@
 namespace navigation {
 
     Debugger::Debugger() {
+        map_max_rows = 1000;
+        map_max_cols = 1000;
         node_handle.getParam("debugger/map_max_rows", map_max_rows);
         node_handle.getParam("debugger/map_max_cols", map_max_cols);
 
-        fusion_map_subscriber = node_handle.subscribe("data_fuser/map", 10, &Debugger::updateFusionMap, this);
+        fusion_map_subscriber = node_handle.subscribe("/data_fuser/map", 10, &Debugger::updateFusionMap, this);
         target_subscriber = node_handle.subscribe("strategy_planner/target", 10, &Debugger::updateTargetPose, this);
         path_subscriber = node_handle.subscribe("local_planner/path", 10, &Debugger::updatePath, this);
 
@@ -72,7 +74,7 @@ int main(int argc, char* argv[]) {
     navigation::Debugger debugger;
     debugger.node_handle = node_handle;
 
-    int loop_rate_hz;
+    int loop_rate_hz = 10;
     node_handle.getParam("debugger/loop_rate", loop_rate_hz);
     ros::Rate loop_rate(loop_rate_hz);
     while (ros::ok()) {

planning/strategy_planner/include/strategy_planner/strategy_planner.hpp

@@ -37,7 +37,7 @@ class Strategy_Planner {
     bool nml_flag, is_confident_;
 
 public:
-    void plan();
+    void plan(bool is_test_mode, int planner, int navigator);
     void setEmergency(std_msgs::String status);
     void setFinalTarget(geometry_msgs::Pose2D set_target_);
     void setWhichPlanner(std::string planner);

planning/strategy_planner/launch/module.launch

@@ -1,5 +1,9 @@
 <?xml version="1.0"?>
 
 <launch>
-    <node pkg="strategy_planner" type="strategy_planner" name="strategy_planner" output="screen" cwd="node" />
+    <node pkg="strategy_planner" type="strategy_planner" name="strategy_planner"  output="screen" cwd="node">
+        <param name="test_mode" type="bool" value="false"/>
+        <param name="planner" type="int" value="0"/>                
+        <param name="navigator" type="int" value="1"/>        
+    </node>
 </launch>

planning/strategy_planner/src/strategy_main.cpp

@@ -3,6 +3,8 @@
 int main(int argc, char* argv[]) {
     Strategy_Planner strategy_planner;
 
+    int planner,navigator;
+    bool is_test_mode;
     ros::init(argc, argv, "strategy_planner");
     ros::NodeHandle nh;
 
@@ -22,7 +24,10 @@ int main(int argc, char* argv[]) {
     ros::Rate loop_rate(loop_rate_hz);
     while (ros::ok()) {
         ros::spinOnce();
-        strategy_planner.plan();
+        nh.getParam("/strategy_planner/test_mode",is_test_mode);
+        nh.getParam("/strategy_planner/planner",planner);
+        nh.getParam("/strategy_planner/navigator",navigator);
+        strategy_planner.plan(is_test_mode,planner,navigator);
         pub_target.publish(strategy_planner.getFinalTarget());
         pub_strategy.publish(strategy_planner.getWhichPlanner());
         pub_navigator.publish(strategy_planner.getWhichNavigator());

planning/strategy_planner/src/strategy_planner.cpp

@@ -80,19 +80,24 @@ void Strategy_Planner::setNavigator(int navigator) {
 
 }
 
-void Strategy_Planner::plan() {
-    if (!is_emergency_) {
-        setPlanner(a_star_seed);
-    } else {
-        setPlanner(quick_response);
-    }
-    if (nml_flag) {
-        setNavigator(waypoint_navigator);
-    } else {
-        if (is_confident_) {
-            setNavigator(lane_navigator);
+void Strategy_Planner::plan(bool is_test_mode, int planner, int navigator) {
+    if (!is_test_mode) {
+        if (!is_emergency_) {
+            setPlanner(a_star_seed);
         } else {
+            setPlanner(quick_response);
+        }
+        if (nml_flag) {
             setNavigator(waypoint_navigator);
+        } else {
+            if (is_confident_) {
+                setNavigator(lane_navigator);
+            } else {
+                setNavigator(waypoint_navigator);
+            }
         }
+    }else {
+        setPlanner(planner);
+        setNavigator(navigator);
     }
 }