Skip to content

Commit a5a5501

Browse files
vernwalrahulamericast
vernwalrahul
authored and
americast
committed
integratedd with GUI (self_rrt.py)
1 parent 45d9e76 commit a5a5501

File tree

3 files changed

+264
-5
lines changed

3 files changed

+264
-5
lines changed

‎CMakeLists.txt‎

Lines changed: 189 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,192 @@ set(SRCS src/Random_sampling.cpp
1616
set(PLANNERS planners)
1717
add_library(${PLANNERS} SHARED ${SRCS})
1818

19-
# Test the planners
20-
set(test_1 test_RT_RRT_star)
21-
add_executable(${test_1} test/test_RT_RRT_star.cpp)
22-
target_link_libraries(${test_1} ${PLANNERS} ${catkin_LIBRARIES})
19+
## Uncomment this if the package has a setup.py. This macro ensures
20+
## modules and global scripts declared therein get installed
21+
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
22+
# catkin_python_setup()
23+
24+
################################################
25+
## Declare ROS messages, services and actions ##
26+
################################################
27+
28+
## To declare and build messages, services or actions from within this
29+
## package, follow these steps:
30+
## * Let MSG_DEP_SET be the set of packages whose message types you use in
31+
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
32+
## * In the file package.xml:
33+
## * add a build_depend tag for "message_generation"
34+
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
35+
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
36+
## but can be declared for certainty nonetheless:
37+
## * add a run_depend tag for "message_runtime"
38+
## * In this file (CMakeLists.txt):
39+
## * add "message_generation" and every package in MSG_DEP_SET to
40+
## find_package(catkin REQUIRED COMPONENTS ...)
41+
## * add "message_runtime" and every package in MSG_DEP_SET to
42+
## catkin_package(CATKIN_DEPENDS ...)
43+
## * uncomment the add_*_files sections below as needed
44+
## and list every .msg/.srv/.action file to be processed
45+
## * uncomment the generate_messages entry below
46+
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
47+
48+
## Generate messages in the 'msg' folder
49+
# add_message_files(
50+
#FILES
51+
# Message1.msg
52+
# Message2.msg
53+
# )
54+
55+
## Generate services in the 'srv' folder
56+
# add_service_files(
57+
# FILES
58+
# Service1.srv
59+
# Service2.srv
60+
# )
61+
62+
## Generate actions in the 'action' folder
63+
# add_action_files(
64+
# FILES
65+
# Action1.action
66+
# Action2.action
67+
# )
68+
69+
## Generate added messages and services with any dependencies listed here
70+
generate_messages(
71+
DEPENDENCIES
72+
# std_msgs # Or other packages containing msgs
73+
)
74+
75+
################################################
76+
## Declare ROS dynamic reconfigure parameters ##
77+
################################################
78+
79+
## To declare and build dynamic reconfigure parameters within this
80+
## package, follow these steps:
81+
## * In the file package.xml:
82+
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
83+
## * In this file (CMakeLists.txt):
84+
## * add "dynamic_reconfigure" to
85+
## find_package(catkin REQUIRED COMPONENTS ...)
86+
## * uncomment the "generate_dynamic_reconfigure_options" section below
87+
## and list every .cfg file to be processed
88+
89+
## Generate dynamic reconfigure parameters in the 'cfg' folder
90+
# generate_dynamic_reconfigure_options(
91+
# cfg/DynReconf1.cfg
92+
# cfg/DynReconf2.cfg
93+
# )
94+
95+
###################################
96+
## catkin specific configuration ##
97+
###################################
98+
## The catkin_package macro generates cmake config files for your package
99+
## Declare things to be passed to dependent projects
100+
## INCLUDE_DIRS: uncomment this if you package contains header files
101+
## LIBRARIES: libraries you create in this project that dependent projects also need
102+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
103+
## DEPENDS: system dependencies of this project that dependent projects also need
104+
catkin_package(
105+
# INCLUDE_DIRS include
106+
# LIBRARIES gui_path
107+
# CATKIN_DEPENDS other_catkin_pkg
108+
# DEPENDS system_lib
109+
INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS}
110+
LIBRARIES ${OpenCV_LIBS}
111+
112+
)
113+
114+
###########
115+
## Build ##
116+
###########
117+
118+
## Specify additional locations of header files
119+
## Your package locations should be listed before other locations
120+
# include_directories(include)
121+
122+
## Declare a C++ library
123+
# add_library(${PROJECT_NAME}
124+
# src/${PROJECT_NAME}/gui_path.cpp
125+
# )
126+
127+
## Add cmake target dependencies of the library
128+
## as an example, code may need to be generated before libraries
129+
## either from message generation or dynamic reconfigure
130+
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
131+
132+
## Declare a C++ executable
133+
## With catkin_make all packages are built within a single CMake context
134+
## The recommended prefix ensures that target names across packages don't collide
135+
# add_executable(${PROJECT_NAME}_node src/gui_path_node.cpp)
136+
137+
## Rename C++ executable without prefix
138+
## The above recommended prefix causes long target names, the following renames the
139+
## target back to the shorter version for ease of user use
140+
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
141+
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
142+
143+
## Add cmake target dependencies of the executable
144+
## same as for the library above
145+
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
146+
147+
## Specify libraries to link a library or executable target against
148+
# target_link_libraries(${PROJECT_NAME}_node
149+
# ${catkin_LIBRARIES}
150+
# )
151+
152+
#############
153+
## Install ##
154+
#############
155+
156+
# all install targets should use catkin DESTINATION variables
157+
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
158+
159+
## Mark executable scripts (Python etc.) for installation
160+
## in contrast to setup.py, you can choose the destination
161+
# install(PROGRAMS
162+
# scripts/my_python_script
163+
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
164+
# )
165+
166+
## Mark executables and/or libraries for installation
167+
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
168+
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
169+
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
170+
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
171+
# )
172+
173+
## Mark cpp header files for installation
174+
# install(DIRECTORY include/${PROJECT_NAME}/
175+
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
176+
# FILES_MATCHING PATTERN "*.h"
177+
# PATTERN ".svn" EXCLUDE
178+
# )
179+
180+
## Mark other files for installation (e.g. launch and bag files, etc.)
181+
# install(FILES
182+
# # myfile1
183+
# # myfile2
184+
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
185+
# )
186+
include_directories(
187+
${catkin_INCLUDE_DIRS}
188+
)
189+
190+
include_directories(include)
191+
192+
#############
193+
## Testing ##
194+
#############
195+
196+
## Add gtest based cpp test target and link libraries
197+
# catkin_add_gtest(${PROJECT_NAME}-test test/test_gui_path.cpp)
198+
# if(TARGET ${PROJECT_NAME}-test)
199+
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
200+
# endif()
201+
202+
## Add folders to be run by python nosetests
203+
# catkin_add_nosetests(test)
204+
set(target self_rrt)
205+
add_executable(${target} src/test_RT_RRT_star.cpp)
206+
message("#####################${catkin_LIBRARIES} ${catkin_INCLUDE_DIRS}")
207+
target_link_libraries(${target} ${catkin_LIBRARIES})

‎src/RT_RRT_star.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ namespace rrt {
99
goal_radius = 20.0;
1010
obstacle_radius = 20.0;
1111
search_radius = 20.0;
12-
node_thresh = 20.0;
12+
node_thresh = 160.0;
1313
epsilon_radius = 20.0;
1414
k_max = 20.0;
1515

‎src/test_RT_RRT_star.cpp‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
//#include "ros/ros.h"
2+
#include "RT_RRT_star.cpp"
3+
#include "ros/ros.h"
4+
#include "krssg_ssl_msgs/planner_path.h"
5+
#include "krssg_ssl_msgs/BeliefState.h"
6+
#include "krssg_ssl_msgs/point_2d.h"
7+
#include "krssg_ssl_msgs/point_SF.h"
8+
using namespace rrt;
9+
using namespace std;
10+
11+
int p=0;
12+
ros::Publisher pub;
13+
14+
void send_points(std::pair<Utils::Point<int>,Utils::Point<int> > child_parent)
15+
{
16+
krssg_ssl_msgs::planner_path points;
17+
18+
krssg_ssl_msgs::point_2d point;
19+
20+
21+
point.x=child_parent.first.x;
22+
point.y=child_parent.first.y;
23+
points.point_array.push_back(point);
24+
25+
point.x=child_parent.second.x;
26+
point.y=child_parent.second.y;
27+
points.point_array.push_back(point);
28+
29+
pub.publish(points); cout<<"data published "<<p++<<endl;
30+
}
31+
32+
int main(int argc, char **argv) {
33+
34+
ros::init(argc, argv, "self_rrt");
35+
ros::NodeHandle n;
36+
pub = n.advertise<krssg_ssl_msgs::planner_path>("self_rrt_pair", 1000);
37+
srand(time(NULL));
38+
Utils::Point<int> start,finish;
39+
40+
start.x=10;
41+
start.y=10;
42+
finish.x=6000;
43+
finish.y=4000;
44+
45+
RT_RRT<int> test(start,finish);
46+
// test.setEndPoints(start,finish);
47+
// test.setCheckPointFunction(*(check));
48+
// test.setStepLength(200);
49+
// test.setHalfDimensions(3000.0,2000.0);
50+
// test.setBiasParameter(100);
51+
// test.setOrigin(origin);
52+
// test.setMaxIterations(10000);
53+
// test.plan();
54+
std::cout<<"#################################################"<<std::endl;
55+
int x=1000;
56+
while(x--)
57+
{
58+
Utils::Point<int> now;
59+
now.x = rand()%6000;
60+
now.y = rand()%4000;
61+
std::pair<Utils::Point<int>,Utils::Point<int> > came_back = test.add_node_to_tree(now);
62+
// test.add_node_to_tree(now);
63+
send_points(came_back);
64+
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;
65+
ros::Duration(0.10).sleep();
66+
// std::cout<<"Generated point\n";
67+
}
68+
// for(int i=0;i<path.size();i++)
69+
// cout<<path[i].x<<","<<path[i].y<<endl;
70+
//ros::init(argc, argv, "RT_RRT_star");
71+
72+
//ros::NodeHandle nh;
73+
74+
}

0 commit comments

Comments
 (0)