diff --git a/applications/eklavya3/eklavya3_roboteq/launch/module.launch b/applications/eklavya3/eklavya3_roboteq/launch/module.launch index 9f140d6..03fda8c 100644 --- a/applications/eklavya3/eklavya3_roboteq/launch/module.launch +++ b/applications/eklavya3/eklavya3_roboteq/launch/module.launch @@ -1,5 +1,5 @@ - + diff --git a/environment/interpretation/data_fuser/src/main.cpp b/environment/interpretation/data_fuser/src/main.cpp index 2b425b0..6024a63 100644 --- a/environment/interpretation/data_fuser/src/main.cpp +++ b/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()); diff --git a/environment/interpretation/lane_detector/launch/module.launch b/environment/interpretation/lane_detector/launch/module.launch index 3c42d87..ac1159d 100644 --- a/environment/interpretation/lane_detector/launch/module.launch +++ b/environment/interpretation/lane_detector/launch/module.launch @@ -1,7 +1,7 @@ - + diff --git a/environment/interpretation/obstacle_detector/src/ObstacleDetector.cpp b/environment/interpretation/obstacle_detector/src/ObstacleDetector.cpp index 61a46ed..5184eed 100644 --- a/environment/interpretation/obstacle_detector/src/ObstacleDetector.cpp +++ b/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() { diff --git a/environment/interpretation/waypoint_selector/CMakeLists.txt b/environment/interpretation/waypoint_selector/CMakeLists.txt index 788e1c7..a093c10 100644 --- a/environment/interpretation/waypoint_selector/CMakeLists.txt +++ b/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) \ No newline at end of file + src/waypoint_selector.cpp) +rosbuild_add_executable(waypoint_recorder + src/waypoint_recorder.cpp) \ No newline at end of file diff --git a/environment/interpretation/waypoint_selector/launch/recorder.launch b/environment/interpretation/waypoint_selector/launch/recorder.launch new file mode 100644 index 0000000..837290d --- /dev/null +++ b/environment/interpretation/waypoint_selector/launch/recorder.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/environment/interpretation/waypoint_selector/src/waypoint_recorder.cpp b/environment/interpretation/waypoint_selector/src/waypoint_recorder.cpp new file mode 100644 index 0000000..3f61c65 --- /dev/null +++ b/environment/interpretation/waypoint_selector/src/waypoint_recorder.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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 ::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(); +} \ No newline at end of file diff --git a/environment/sensing/vn_ins/src/vn_ins.cpp b/environment/sensing/vn_ins/src/vn_ins.cpp index 8e5055f..d0be088 100644 --- a/environment/sensing/vn_ins/src/vn_ins.cpp +++ b/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,12 +40,12 @@ 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; @@ -53,7 +53,7 @@ bool vectorNav::fetch() { } void vectorNav::publish(int frame_id) { - //fix_publisher.publish(_gps); + fix_publisher.publish(_gps); yaw_publisher.publish(_yaw); } @@ -61,9 +61,9 @@ 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(fix_topic_name.c_str(), message_queue_size); + fix_publisher = node_handle->advertise(fix_topic_name.c_str(), message_queue_size); yaw_publisher = node_handle->advertise(yaw_topic_name.c_str(), message_queue_size); } diff --git a/planning/motion_planning/local_planner/launch/module.launch b/planning/motion_planning/local_planner/launch/module.launch index e60c7ba..e5b2dd7 100644 --- a/planning/motion_planning/local_planner/launch/module.launch +++ b/planning/motion_planning/local_planner/launch/module.launch @@ -5,7 +5,7 @@ - + @@ -19,10 +19,5 @@ - - - - - diff --git a/planning/motion_planning/local_planner/src/local_planner.cpp b/planning/motion_planning/local_planner/src/local_planner.cpp index ea68e82..61616ab 100644 --- a/planning/motion_planning/local_planner/src/local_planner.cpp +++ b/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); diff --git a/planning/motion_planning/local_planner/src/local_planner/__init__.pyc b/planning/motion_planning/local_planner/src/local_planner/__init__.pyc index c2fcbea..91640a5 100644 Binary files a/planning/motion_planning/local_planner/src/local_planner/__init__.pyc and b/planning/motion_planning/local_planner/src/local_planner/__init__.pyc differ diff --git a/planning/motion_planning/showpath_debugger/src/debugger.cpp b/planning/motion_planning/showpath_debugger/src/debugger.cpp index 24e3663..f28ebcc 100644 --- a/planning/motion_planning/showpath_debugger/src/debugger.cpp +++ b/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()) { diff --git a/planning/strategy_planner/include/strategy_planner/strategy_planner.hpp b/planning/strategy_planner/include/strategy_planner/strategy_planner.hpp index 0d45909..d518e07 100644 --- a/planning/strategy_planner/include/strategy_planner/strategy_planner.hpp +++ b/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); diff --git a/planning/strategy_planner/launch/module.launch b/planning/strategy_planner/launch/module.launch index 3dcde5e..b036c94 100644 --- a/planning/strategy_planner/launch/module.launch +++ b/planning/strategy_planner/launch/module.launch @@ -1,5 +1,9 @@ - + + + + + diff --git a/planning/strategy_planner/src/strategy_main.cpp b/planning/strategy_planner/src/strategy_main.cpp index cea660f..fd94587 100644 --- a/planning/strategy_planner/src/strategy_main.cpp +++ b/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()); diff --git a/planning/strategy_planner/src/strategy_planner.cpp b/planning/strategy_planner/src/strategy_planner.cpp index 6e70a07..3f190be 100644 --- a/planning/strategy_planner/src/strategy_planner.cpp +++ b/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); } }