Skip to content

Commit c7000eb

Browse files
RaventhatflyRaventhatfly
committed
hero related files
1 parent 27d4030 commit c7000eb

File tree

8 files changed

+459
-11
lines changed

8 files changed

+459
-11
lines changed

‎decision/hero_vehicle/CMakeLists.txt‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(hero_vehicle)
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+
find_package(ament_cmake_auto REQUIRED)
11+
ament_auto_find_build_dependencies()
12+
13+
ament_auto_add_library(${PROJECT_NAME} SHARED
14+
src/hero_vehicle.cpp
15+
src/dbus_interpreter.cpp)
16+
17+
rclcpp_components_register_node(${PROJECT_NAME}
18+
PLUGIN "HeroVehicle"
19+
EXECUTABLE ${PROJECT_NAME}_node)
20+
21+
if(BUILD_TESTING)
22+
find_package(ament_lint_auto REQUIRED)
23+
# the following line skips the linter which checks for copyrights
24+
# comment the line when a copyright and license is added to all source files
25+
set(ament_cmake_copyright_FOUND TRUE)
26+
# the following line skips cpplint (only works in a git repo)
27+
# comment the line when this package is in a git repo and when
28+
# a copyright and license is added to all source files
29+
set(ament_cmake_cpplint_FOUND TRUE)
30+
ament_lint_auto_find_test_dependencies()
31+
endif()
32+
33+
ament_auto_package()

‎decision/hero_vehicle/README.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
# Hero Vehicle
2+
3+
This node is active when dbus left switch is at the MIDDLE.
4+
5+
It can be launched with one of the following:
6+
1. km_vehicle
7+
2. auto_sentry

‎decision/hero_vehicle/include/hero_vehicle/dbus_interpreter.h‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
#ifndef DBUS_INTERPRETER_H
2+
#define DBUS_INTERPRETER_H
3+
4+
#include "rclcpp/rclcpp.hpp"
5+
#include <cstdint>
6+
#include <thread>
7+
8+
#include "operation_interface/msg/dbus_control.hpp"
9+
#include "operation_interface/msg/key_mouse.hpp"
10+
#include "behavior_interface/msg/move.hpp"
11+
#include "behavior_interface/msg/shoot.hpp"
12+
#include "behavior_interface/msg/aim.hpp"
13+
#include "behavior_interface/msg/chassis.hpp"
14+
#include "geometry_msgs/msg/twist.hpp"
15+
#include <behavior_interface/msg/detail/chassis__struct.hpp>
16+
17+
#define PERIOD 10 // ms
18+
19+
using operation_interface::msg::DbusControl;
20+
using operation_interface::msg::KeyMouse;
21+
using behavior_interface::msg::Move;
22+
using behavior_interface::msg::Shoot;
23+
using behavior_interface::msg::Aim;
24+
using behavior_interface::msg::Chassis;
25+
26+
class DbusInterpreter
27+
{
28+
public:
29+
DbusInterpreter(double max_vel, double max_omega, double aim_sens, double deadzone);
30+
31+
~DbusInterpreter();
32+
33+
void input(const DbusControl::SharedPtr msg);
34+
35+
void input_key(const KeyMouse::SharedPtr msg);
36+
37+
Move::SharedPtr get_move() const;
38+
geometry_msgs::msg::Twist get_move_ros2_control() const;
39+
40+
Shoot::SharedPtr get_shoot() const;
41+
42+
Aim::SharedPtr get_aim() const;
43+
44+
Chassis::SharedPtr get_chassis() const;
45+
46+
bool is_active() const { return active; }
47+
48+
private:
49+
bool active;
50+
51+
double ls_x, ls_y, rs_x, rs_y, wheel;
52+
std::string lsw, rsw;
53+
double mouse_x_ = 0.0, mouse_y_ = 0.0, mouse_z_ = 0.0;
54+
bool left_button_ = false, right_button_ = false;
55+
bool w_ = false, a_ = false, s_ = false;
56+
bool d_ = false, shift_ = false, ctrl_ = false;
57+
bool q_ = false, e_ = false, r_ = false;
58+
bool f_ = false, g_ = false, z_ = false;
59+
bool x_ = false, c_ = false, v_, b_ = false;
60+
bool last_c_;
61+
62+
rclcpp::Time last_update_time_;
63+
64+
double max_vel, max_omega, max_feed, max_shoot, aim_sens, deadzone;
65+
66+
std::thread update_thread;
67+
68+
Move::SharedPtr move_;
69+
Shoot::SharedPtr shoot_;
70+
Aim::SharedPtr aim_;
71+
Chassis::SharedPtr chassis_;
72+
73+
void update();
74+
75+
void apply_deadzone(double& val);
76+
77+
void curb(double& val, double max_val);
78+
};
79+
80+
#endif // DBUS_INTERPRETER_H

‎decision/hero_vehicle/package.xml‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
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>hero_vehicle</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="yao29@illinois.edu">Xinchen Yao</maintainer>
8+
<maintainer email="chiming2@illinois.edu">Chiming Ni</maintainer>
9+
<maintainer email="raventhatfly@gmail.com">Feiyang Wu</maintainer>
10+
<license>TODO: License declaration</license>
11+
12+
<buildtool_depend>ament_cmake</buildtool_depend>
13+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
14+
15+
<test_depend>ament_lint_auto</test_depend>
16+
<test_depend>ament_lint_common</test_depend>
17+
18+
<depend>operation_interface</depend>
19+
<depend>geometry_msgs</depend>
20+
<depend>behavior_interface</depend>
21+
<depend>rclcpp</depend>
22+
<depend>rclcpp_components</depend>
23+
24+
<export>
25+
<build_type>ament_cmake</build_type>
26+
</export>
27+
</package>

‎decision/hero_vehicle/src/dbus_interpreter.cpp‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
#include "hero_vehicle/dbus_interpreter.h"
2+
#include <cmath>
3+
4+
DbusInterpreter::DbusInterpreter(double max_vel, double max_omega, double aim_sens, double deadzone)
5+
: max_vel(max_vel), max_omega(max_omega), aim_sens(aim_sens), deadzone(deadzone)
6+
{
7+
// initialize buttons and axes
8+
active = false;
9+
ls_x = ls_y = rs_x = rs_y = wheel = 0;
10+
lsw = rsw = "";
11+
12+
// initialize move, shoot, aim, and chassis state
13+
move_ = std::make_shared<Move>();
14+
shoot_ = std::make_shared<Shoot>();
15+
aim_ = std::make_shared<Aim>();
16+
chassis_ = std::make_shared<Chassis>();
17+
18+
// initialize chassis mode
19+
chassis_->mode = behavior_interface::msg::Chassis::CHASSIS;
20+
21+
// Last Update Time
22+
last_update_time_ = rclcpp::Clock().now();
23+
last_c_ = false;
24+
25+
// initialize update thread
26+
update_thread = std::thread([this](){
27+
while (rclcpp::ok())
28+
{
29+
update();
30+
std::this_thread::sleep_for(std::chrono::milliseconds(PERIOD));
31+
}
32+
});
33+
}
34+
35+
DbusInterpreter::~DbusInterpreter()
36+
{
37+
if (update_thread.joinable()) update_thread.join();
38+
}
39+
40+
void DbusInterpreter::input(const operation_interface::msg::DbusControl::SharedPtr msg)
41+
{
42+
ls_x = msg->ls_x; apply_deadzone(ls_x); // forward is positive
43+
ls_y = msg->ls_y; apply_deadzone(ls_y); // left is positive
44+
rs_x = msg->rs_x; apply_deadzone(rs_x); // up is positive
45+
rs_y = msg->rs_y; apply_deadzone(rs_y); // left is positive
46+
wheel = msg->wheel; apply_deadzone(wheel);
47+
lsw = msg->lsw;
48+
rsw = msg->rsw;
49+
}
50+
51+
void DbusInterpreter::input_key(const operation_interface::msg::KeyMouse::SharedPtr msg)
52+
{
53+
w_ = msg->w;
54+
a_ = msg->a;
55+
s_ = msg->s;
56+
d_ = msg->d;
57+
shift_ = msg->shift;
58+
ctrl_ = msg->ctrl;
59+
q_ = msg->q;
60+
e_ = msg->e;
61+
r_ = msg->r;
62+
f_ = msg->f;
63+
g_ = msg->g;
64+
z_ = msg->z;
65+
x_ = msg->x;
66+
c_ = msg->c;
67+
v_ = msg->v;
68+
b_ = msg->b;
69+
left_button_ = msg->left_button;
70+
right_button_ = msg->right_button;
71+
mouse_x_ = msg->mouse_x;
72+
mouse_y_ = msg->mouse_y;
73+
}
74+
75+
void DbusInterpreter::update()
76+
{
77+
active = (lsw == "MID");
78+
if (!active)
79+
{
80+
return; // do not update if not active, this prevents yaw and pitch from accumulating in standby
81+
}
82+
83+
move_->vel_x = max_vel * ls_x;
84+
move_->vel_y = max_vel * ls_y;
85+
aim_->pitch += aim_sens * rs_x * PERIOD / 1000; curb(aim_->pitch, M_PI_4);
86+
move_->omega = max_omega * wheel;
87+
aim_->yaw += aim_sens * rs_y * PERIOD / 1000;
88+
89+
if (rsw == "UP")
90+
{
91+
shoot_->fric_state = false;
92+
shoot_->feed_state = false;
93+
shoot_->feed_speed = 0;
94+
}
95+
else if (rsw == "MID")
96+
{
97+
shoot_->fric_state = true;
98+
if(left_button_){
99+
shoot_->feed_state = true;
100+
shoot_->feed_speed = 5.0;
101+
}else{
102+
shoot_->feed_state = false;
103+
shoot_->feed_speed = 0.0;
104+
}
105+
}
106+
else if (rsw == "DOWN")
107+
{
108+
shoot_->fric_state = true;
109+
shoot_->feed_state = true;
110+
shoot_->feed_speed = 5.0;
111+
}
112+
113+
114+
// TODO: Implement Keyboard Actions
115+
int move_x = 0, move_y = 0;
116+
if(w_) move_x += max_vel;
117+
if(s_) move_x -= max_vel;
118+
move_->vel_x += move_x;
119+
120+
if(a_) move_y += max_vel;
121+
if(d_) move_y -= max_vel;
122+
move_->vel_y += move_y;
123+
124+
aim_->yaw -= mouse_x_ * aim_sens * PERIOD / 200;
125+
aim_->pitch -= mouse_y_ * aim_sens * PERIOD / 200; curb(aim_->pitch, M_PI_4);
126+
if(q_) aim_->yaw += aim_sens * 0.5 * PERIOD / 1000;
127+
if(e_) aim_->yaw -= aim_sens * 0.5 * PERIOD / 1000;
128+
129+
// To ensure that the change take place only once per key press
130+
auto current_time = rclcpp::Clock().now();
131+
132+
// if(current_time.seconds()-last_update_time_.seconds() > 0.2){
133+
// if(c_ && !last_c_) // TOGGLE CHASSIS MODE
134+
// {
135+
// if(chassis_->mode == behavior_interface::msg::Chassis::GYRO){
136+
// chassis_->mode = behavior_interface::msg::Chassis::CHASSIS_FOLLOW;
137+
// }else if(chassis_->mode = behavior_interface::msg::Chassis::CHASSIS_FOLLOW){
138+
// chassis_->mode = behavior_interface::msg::Chassis::GYRO;
139+
// }
140+
// }
141+
// last_update_time_ = rclcpp::Clock().now();
142+
// }
143+
144+
// if(c_ && !last_c_){
145+
// if(chassis_->mode == behavior_interface::msg::Chassis::GYRO){
146+
// chassis_->mode = behavior_interface::msg::Chassis::CHASSIS_FOLLOW;
147+
// }else if(chassis_->mode = behavior_interface::msg::Chassis::CHASSIS_FOLLOW){
148+
// chassis_->mode = behavior_interface::msg::Chassis::GYRO;
149+
// }
150+
// }
151+
last_c_ = c_;
152+
// if(ctrl_){
153+
// chassis_->mode = behavior_interface::msg::Chassis::CHASSIS_FOLLOW;
154+
// }
155+
156+
157+
158+
}
159+
160+
void DbusInterpreter::apply_deadzone(double &val)
161+
{
162+
if (val < deadzone && val > -deadzone)
163+
{
164+
val = 0;
165+
}
166+
}
167+
168+
Move::SharedPtr DbusInterpreter::get_move() const
169+
{
170+
return move_;
171+
}
172+
173+
geometry_msgs::msg::Twist DbusInterpreter::get_move_ros2_control() const
174+
{
175+
geometry_msgs::msg::Twist move_msg_ros2_control;
176+
move_msg_ros2_control.linear.x = move_->vel_x;
177+
move_msg_ros2_control.linear.y = move_->vel_y;
178+
move_msg_ros2_control.angular.z = move_->omega;
179+
return move_msg_ros2_control;
180+
}
181+
182+
Shoot::SharedPtr DbusInterpreter::get_shoot() const
183+
{
184+
return shoot_;
185+
}
186+
187+
Aim::SharedPtr DbusInterpreter::get_aim() const
188+
{
189+
return aim_;
190+
}
191+
192+
Chassis::SharedPtr DbusInterpreter::get_chassis() const
193+
{
194+
return chassis_;
195+
}
196+
197+
void DbusInterpreter::curb(double &val, double max_val)
198+
{
199+
if (val > max_val)
200+
{
201+
val = max_val;
202+
}
203+
else if (val < -max_val)
204+
{
205+
val = -max_val;
206+
}
207+
}

0 commit comments

Comments
 (0)