integratedd with GUI (self_rrt.py)

CMakeLists.txt

@@ -16,7 +16,192 @@ set(SRCS src/Random_sampling.cpp
 set(PLANNERS planners)
 add_library(${PLANNERS} SHARED ${SRCS})
 
-# Test the planners
-set(test_1 test_RT_RRT_star)
-add_executable(${test_1} test/test_RT_RRT_star.cpp)
-target_link_libraries(${test_1} ${PLANNERS} ${catkin_LIBRARIES})
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES gui_path
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+    INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS}
+    LIBRARIES ${OpenCV_LIBS}
+
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/gui_path.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/gui_path_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+include_directories(include)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_gui_path.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+set(target self_rrt)
+add_executable(${target} src/test_RT_RRT_star.cpp)
+message("#####################${catkin_LIBRARIES} ${catkin_INCLUDE_DIRS}")
+target_link_libraries(${target} ${catkin_LIBRARIES})

src/RT_RRT_star.cpp

@@ -9,7 +9,7 @@ namespace rrt {
     goal_radius = 20.0;
     obstacle_radius = 20.0;
     search_radius = 20.0;
-    node_thresh = 20.0;
+    node_thresh = 160.0;
     epsilon_radius = 20.0;
     k_max = 20.0;
 

src/test_RT_RRT_star.cpp

@@ -0,0 +1,74 @@
+//#include "ros/ros.h"
+#include "RT_RRT_star.cpp"
+#include "ros/ros.h"
+#include "krssg_ssl_msgs/planner_path.h"
+#include "krssg_ssl_msgs/BeliefState.h"
+#include "krssg_ssl_msgs/point_2d.h"
+#include "krssg_ssl_msgs/point_SF.h"
+using namespace rrt;
+using namespace std;
+
+int p=0;
+ros::Publisher pub;
+
+void send_points(std::pair<Utils::Point<int>,Utils::Point<int> > child_parent)
+{
+    krssg_ssl_msgs::planner_path points;
+
+    krssg_ssl_msgs::point_2d point;
+
+
+        point.x=child_parent.first.x;
+        point.y=child_parent.first.y;
+        points.point_array.push_back(point);
+
+        point.x=child_parent.second.x;
+        point.y=child_parent.second.y;
+        points.point_array.push_back(point);
+
+      pub.publish(points); cout<<"data published "<<p++<<endl;
+}
+
+int main(int argc, char **argv) {
+
+    ros::init(argc, argv, "self_rrt");
+    ros::NodeHandle n;
+    pub = n.advertise<krssg_ssl_msgs::planner_path>("self_rrt_pair", 1000);
+    srand(time(NULL));
+    Utils::Point<int> start,finish;
+
+    start.x=10;
+    start.y=10;
+    finish.x=6000;
+    finish.y=4000;
+
+    RT_RRT<int> test(start,finish);
+    // test.setEndPoints(start,finish);
+    // test.setCheckPointFunction(*(check));
+    // test.setStepLength(200);
+    // test.setHalfDimensions(3000.0,2000.0);
+    // test.setBiasParameter(100);
+    // test.setOrigin(origin);
+    // test.setMaxIterations(10000);
+    // test.plan();
+    std::cout<<"#################################################"<<std::endl;
+    int x=1000;
+    while(x--)
+    {
+        Utils::Point<int> now;
+        now.x = rand()%6000;
+        now.y = rand()%4000;
+        std::pair<Utils::Point<int>,Utils::Point<int> > came_back = test.add_node_to_tree(now);
+        // test.add_node_to_tree(now);
+        send_points(came_back);
+        std::cout<<"Generated a point "<<came_back.first.x<<","<<came_back.first.y<<" with parent "<<came_back.second.x<<","<<came_back.second.y<<std::endl;
+        ros::Duration(0.10).sleep();
+        // std::cout<<"Generated point\n";
+    }
+    // for(int i=0;i<path.size();i++)
+    //     cout<<path[i].x<<","<<path[i].y<<endl;
+  //ros::init(argc, argv, "RT_RRT_star");
+
+  //ros::NodeHandle nh;
+
+}