Skip to content

Commit c881f11

Browse files
Thaumaturge2020Thaumaturge2020
committed
最初版得玩具
0 parents  commit c881f11

31 files changed

+2119
-0
lines changed

‎src/behavior_decision/CMakeLists.txt

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(behavior_decision)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
9+
if(NOT ROOT)
10+
set(ROOT ${PROJECT_SOURCE_DIR}/)
11+
endif()
12+
13+
add_definitions(-DROOT="${ROOT}")
14+
15+
find_package(OpenCV REQUIRED)
16+
17+
# find dependencies
18+
find_package(ament_cmake REQUIRED)
19+
find_package(rclcpp REQUIRED)
20+
find_package(rclcpp_action REQUIRED)
21+
find_package(rclcpp_components REQUIRED)
22+
find_package(std_msgs REQUIRED)
23+
find_package(task_msg REQUIRED)
24+
find_package(OpenCV REQUIRED)
25+
include_directories("/usr/include/eigen3")
26+
27+
if(BUILD_TESTING)
28+
find_package(ament_lint_auto REQUIRED)
29+
# the following line skips the linter which checks for copyrights
30+
# comment the line when a copyright and license is added to all source files
31+
set(ament_cmake_copyright_FOUND TRUE)
32+
# the following line skips cpplint (only works in a git repo)
33+
# comment the line when this package is in a git repo and when
34+
# a copyright and license is added to all source files
35+
set(ament_cmake_cpplint_FOUND TRUE)
36+
ament_lint_auto_find_test_dependencies()
37+
endif()
38+
39+
add_library(behavior_decision SHARED src/btree.cpp)
40+
41+
target_include_directories(behavior_decision PRIVATE
42+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
43+
$<INSTALL_INTERFACE:include>)
44+
45+
ament_target_dependencies(behavior_decision rclcpp rclcpp_action rclcpp_components task_msg std_msgs OpenCV)
46+
47+
ament_package()
48+
49+
rclcpp_components_register_node(behavior_decision PLUGIN "behaviourDecision::BehaviourTree" EXECUTABLE btree)
50+
51+
install(TARGETS
52+
behavior_decision
53+
ARCHIVE DESTINATION lib
54+
LIBRARY DESTINATION lib
55+
RUNTIME DESTINATION bin
56+
)

‎src/behavior_decision/package.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>behavior_decision</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="[email protected]">red-sakura</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<depend>rclcpp</depend>
11+
<depend>task_msg</depend>
12+
<depend>std_msgs</depend>
13+
14+
<buildtool_depend>ament_cmake</buildtool_depend>
15+
16+
<test_depend>ament_lint_auto</test_depend>
17+
<test_depend>ament_lint_common</test_depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
</export>
22+
</package>

‎src/behavior_decision/src/btree.cpp

+55
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
#include <rclcpp/rclcpp.hpp>
2+
#include <rclcpp_components/register_node_macro.hpp>
3+
#include <queue>
4+
#include <Eigen/Core>
5+
#include <Eigen/Dense>
6+
#include <toml.hpp>
7+
#include <utility>
8+
#include <std_msgs/msg/float64.hpp>
9+
#include <std_msgs/msg/int64.hpp>
10+
#include "task_msg/msg/float64_pair_multi_array.hpp"
11+
#include "task_msg/msg/float64_multi_array.hpp"
12+
#include "task_msg/srv/point2d_move.hpp"
13+
#include "task_msg/srv/nav_find.hpp"
14+
15+
#include <opencv2/opencv.hpp>
16+
#include <opencv2/highgui/highgui.hpp>
17+
#include <opencv2/imgproc/imgproc.hpp>
18+
19+
namespace behaviourDecision{
20+
class BehaviourTree:public rclcpp::Node{
21+
public:
22+
rclcpp::Publisher<task_msg::msg::Float64MultiArray>::SharedPtr publisher_global_plan_pos;
23+
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr publisher_shooter;
24+
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_avail_angle;
25+
rclcpp::TimerBase::SharedPtr timer_;
26+
explicit BehaviourTree(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
27+
: Node("behaviour_tree", options){
28+
timer_ = this->create_wall_timer(std::chrono::milliseconds(5), std::bind(&BehaviourTree::behaviour_publish, this));
29+
publisher_global_plan_pos = this->create_publisher<task_msg::msg::Float64MultiArray>("end_point_global_planner",10);
30+
publisher_avail_angle = this->create_publisher<std_msgs::msg::Float64>("decision_shooting_angle",10);
31+
publisher_shooter = this->create_publisher<std_msgs::msg::Int64>("decision_shooting_able",10);
32+
}
33+
void behaviour_publish(){
34+
const double pi = 3.14159265358979323;
35+
std::vector<double> vec;
36+
static double CNT = 0;
37+
while(CNT > 2*pi) CNT -= 2*pi;
38+
double angle = ++CNT;
39+
std_msgs::msg::Float64 pub_angle;
40+
pub_angle.data = angle;
41+
vec.push_back(1.2);
42+
vec.push_back(3.16);
43+
task_msg::msg::Float64MultiArray pub_msg;
44+
pub_msg.data = vec;
45+
publisher_global_plan_pos->publish(pub_msg);
46+
publisher_avail_angle->publish(pub_angle);
47+
std_msgs::msg::Int64 able_msg;
48+
able_msg.data = 1;
49+
publisher_shooter->publish(able_msg);
50+
return;
51+
}
52+
};
53+
}
54+
55+
RCLCPP_COMPONENTS_REGISTER_NODE(behaviourDecision::BehaviourTree)

‎src/bring_up/CMakeLists.txt

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(bring_up)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
# find dependencies
9+
find_package(ament_cmake REQUIRED)
10+
# uncomment the following section in order to fill in
11+
# further dependencies manually.
12+
# find_package(<dependency> REQUIRED)
13+
14+
if(BUILD_TESTING)
15+
find_package(ament_lint_auto REQUIRED)
16+
# the following line skips the linter which checks for copyrights
17+
# comment the line when a copyright and license is added to all source files
18+
set(ament_cmake_copyright_FOUND TRUE)
19+
# the following line skips cpplint (only works in a git repo)
20+
# comment the line when this package is in a git repo and when
21+
# a copyright and license is added to all source files
22+
set(ament_cmake_cpplint_FOUND TRUE)
23+
ament_lint_auto_find_test_dependencies()
24+
endif()
25+
26+
# Install launch files.
27+
install(DIRECTORY
28+
launch
29+
DESTINATION share/${PROJECT_NAME}/
30+
)
31+
32+
ament_package()

‎src/bring_up/launch/freshman_launch.py

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import Node
3+
4+
def generate_launch_description():
5+
return LaunchDescription([
6+
Node(
7+
package="platform",
8+
executable="object",
9+
name="my_platform",
10+
output="screen",
11+
emulate_tty=True
12+
),
13+
Node(
14+
package="global_planner_task",
15+
executable="global_planner",
16+
name="global_planner_1",
17+
output="screen",
18+
emulate_tty=True
19+
),
20+
Node(
21+
package="local_planner_task",
22+
executable="local_planner",
23+
name="local_planner_1",
24+
output="screen",
25+
emulate_tty=True
26+
),
27+
Node(
28+
package="behavior_decision",
29+
executable="btree",
30+
name="behavior_decision_1",
31+
output="screen",
32+
emulate_tty=True
33+
),
34+
])

‎src/bring_up/package.xml

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>bring_up</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="[email protected]">red-sakura</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<test_depend>ament_lint_auto</test_depend>
13+
<test_depend>ament_lint_common</test_depend>
14+
15+
<export>
16+
<build_type>ament_cmake</build_type>
17+
</export>
18+
</package>

‎src/global_planner_task/CMakeLists.txt

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(global_planner_task)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
if(NOT ROOT)
9+
set(ROOT ${PROJECT_SOURCE_DIR}/)
10+
endif()
11+
12+
add_definitions(-DROOT="${ROOT}")
13+
14+
find_package(OpenCV REQUIRED)
15+
16+
# find dependencies
17+
find_package(ament_cmake REQUIRED)
18+
find_package(rclcpp REQUIRED)
19+
find_package(rclcpp_action REQUIRED)
20+
find_package(rclcpp_components REQUIRED)
21+
find_package(std_msgs REQUIRED)
22+
find_package(task_msg REQUIRED)
23+
find_package(OpenCV REQUIRED)
24+
include_directories("/usr/include/eigen3")
25+
# uncomment the following section in order to fill in
26+
# further dependencies manually.
27+
# find_package(<dependency> REQUIRED)
28+
29+
if(BUILD_TESTING)
30+
find_package(ament_lint_auto REQUIRED)
31+
# the following line skips the linter which checks for copyrights
32+
# comment the line when a copyright and license is added to all source files
33+
set(ament_cmake_copyright_FOUND TRUE)
34+
# the following line skips cpplint (only works in a git repo)
35+
# comment the line when this package is in a git repo and when
36+
# a copyright and license is added to all source files
37+
set(ament_cmake_cpplint_FOUND TRUE)
38+
ament_lint_auto_find_test_dependencies()
39+
endif()
40+
41+
add_library(global_planner_task SHARED src/global_planner.cpp)
42+
43+
target_include_directories(global_planner_task PRIVATE
44+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
45+
$<INSTALL_INTERFACE:include>)
46+
47+
ament_target_dependencies(global_planner_task rclcpp rclcpp_action rclcpp_components task_msg std_msgs OpenCV)
48+
49+
ament_package()
50+
51+
rclcpp_components_register_node(global_planner_task PLUGIN "globalPlanningCpp::GlobalPlanner" EXECUTABLE global_planner)
52+
53+
install(TARGETS
54+
global_planner_task
55+
ARCHIVE DESTINATION lib
56+
LIBRARY DESTINATION lib
57+
RUNTIME DESTINATION bin
58+
)

‎src/global_planner_task/include/global_planner/global_planner.hpp

Whitespace-only changes.

‎src/global_planner_task/package.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>global_planner_task</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="[email protected]">red-sakura</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<depend>rclcpp</depend>
11+
<depend>task_msg</depend>
12+
<depend>std_msg</depend>
13+
14+
<buildtool_depend>ament_cmake</buildtool_depend>
15+
16+
<test_depend>ament_lint_auto</test_depend>
17+
<test_depend>ament_lint_common</test_depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
</export>
22+
</package>

‎src/global_planner_task/src/field.toml

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
node_tot_num = 23
2+
3+
edge_able = [
4+
[1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
5+
[1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1],
6+
[0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
7+
[0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
8+
[0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
9+
[0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
10+
[0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0],
11+
[0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
12+
[0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0],
13+
[0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0],
14+
[0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0],
15+
[0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0],
16+
[0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0],
17+
[0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0,0],
18+
[0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0],
19+
[0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0],
20+
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,0,0,0,0],
21+
[0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,1,1,0,0,0,0],
22+
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,0,0,0],
23+
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0],
24+
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,0],
25+
[0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1],
26+
[0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1]
27+
]
28+
29+
point_position = [[2.200000,8.540000],[3.160000,7.240000],[1.020000,5.540000],[0.900000,2.640000],[3.220000,3.860000],[4.560000,5.460000],[6.220000,6.180000],[6.780000,5.120000],[8.280000,4.220000],[6.820000,3.120000],[5.820000,2.160000],[6.120000,0.700000],[9.120000,0.720000],[11.500000,1.440000],[13.320000,1.380000],[13.140000,2.940000],[13.080000,4.680000],[10.600000,5.420000],[12.760000,6.700000],[12.940000,8.780000],[10.180000,8.780000],[6.520000,8.220000],[4.560000,8.700000]]

0 commit comments

Comments
 (0)