Skip to content

Commit 8ae535e

Browse files
yuvrajdhadwalgithub-actions[bot]
yuvrajdhadwal
and
github-actions[bot]
authored
Upgrade line test functionality (#2509)
* now can take in points for line test * got communication from callback to line derived get task * confirmed comms and added comments explaining how to use * added motion, defaults, and cleared logs * added comments explaining code * Fix Code Style On upgrade_line_test_functionality (#2510) automated style fixes Co-authored-by: yuvrajdhadwal <yuvrajdhadwal@users.noreply.github.com> * deleted deprecated code * Fix Code Style On upgrade_line_test_functionality (#2514) automated style fixes Co-authored-by: yuvrajdhadwal <yuvrajdhadwal@users.noreply.github.com> * added bounding checks for points * Fix Code Style On upgrade_line_test_functionality (#2518) automated style fixes Co-authored-by: yuvrajdhadwal <yuvrajdhadwal@users.noreply.github.com> --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: yuvrajdhadwal <yuvrajdhadwal@users.noreply.github.com>
1 parent dd55658 commit 8ae535e

8 files changed

Lines changed: 127 additions & 103 deletions

File tree

‎src/rj_msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ rosidl_generate_interfaces(
3636
msg/MatchState.msg
3737
msg/MotionSetpoint.msg
3838
msg/Latency.msg
39+
msg/LineTest.msg
3940

4041
msg/LinearMotionInstant.msg
4142
msg/MotionCommand.msg

‎src/rj_msgs/msg/LineTest.msg‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
rj_geometry_msgs/Point[2] pt
2+
uint8 r_id

‎src/rj_param_utils/config/real_params.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,12 @@
9191
search_start_dist: 0.0
9292
shortcut_dist: 0.09
9393
target_point_gain: 0.5
94+
line_test:
95+
start_x: 1.0
96+
start_y: 7.0
97+
end_x: -1.0
98+
end_y: 7.0
99+
robot_id: 1
94100
timeout: 0.1
95101
use_sim_time: false
96102

‎src/rj_param_utils/config/sim_params.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,12 @@
9191
search_start_dist: 0.0
9292
shortcut_dist: 0.09
9393
target_point_gain: 0.5
94+
line_test:
95+
start_x: 1.0
96+
start_y: 7.0
97+
end_x: -1.0
98+
end_y: 7.0
99+
robot_id: 1
94100
timeout: 0.1
95101
use_sim_time: false
96102

‎src/rj_strategy/include/rj_strategy/agent/position/line.hpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ class Line : public Position {
1111
Line(Position&& other);
1212
Line(int r_id);
1313
Line(int r_id, bool forward);
14+
Line(int r_id, rj_geometry::Point start, rj_geometry::Point end, uint8_t target_rid);
1415
~Line() override = default;
1516
Line(const Line& other) = default;
1617
Line(Line&& other) = default;
@@ -22,5 +23,9 @@ class Line : public Position {
2223
bool forward_ = true;
2324
bool vertical_ = false;
2425
bool face_target_ = false;
26+
27+
rj_geometry::Point start_;
28+
rj_geometry::Point end_;
29+
uint8_t target_rid_;
2530
};
2631
} // namespace strategy

‎src/rj_strategy/include/rj_strategy/testing/straight_line_test.hpp‎

Lines changed: 27 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2,15 +2,18 @@
22

33
#include <rclcpp/rclcpp.hpp>
44
#include <rclcpp_action/rclcpp_action.hpp>
5+
#include <spdlog/spdlog.h>
56

67
#include <rj_common/game_state.hpp>
78
#include <rj_common/robot_intent.hpp>
89
#include <rj_constants/topic_names.hpp>
10+
#include <rj_geometry/point.hpp>
911
#include <rj_msgs/action/robot_move.hpp>
1012
#include <rj_msgs/msg/agent_state.hpp>
1113
#include <rj_msgs/msg/alive_robots.hpp>
1214
#include <rj_msgs/msg/field_dimensions.hpp>
1315
#include <rj_msgs/msg/game_settings.hpp>
16+
#include <rj_msgs/msg/line_test.hpp>
1417
#include <rj_msgs/msg/play_state.hpp>
1518
#include <rj_msgs/msg/world_state.hpp>
1619
#include <rj_param_utils/global_params.hpp>
@@ -20,8 +23,17 @@
2023
#include "rj_strategy/agent/position.hpp"
2124
#include "rj_strategy/agent/position/line.hpp"
2225

23-
// Note: The direction of the line can be changed by running:
24-
// `ros2 topic pub -1 line_direction std_msgs/msg/Bool "{data: VERTICAL}"`
26+
// run test by using the command `make run-sim-line-test`
27+
// Note: The the line can be changed by running following command in another terminal window:
28+
// Make sure to source bash and ros in other window
29+
// Example command:
30+
// `ros2 topic pub -1 line rj_msgs/msg/LineTest "{pt: [{x: 2, y: 2}, {x: -2, y: 8}], r_id: 2}"`
31+
32+
DECLARE_FLOAT64("straight_line_test", start_x);
33+
DECLARE_FLOAT64("straight_line_test", start_y);
34+
DECLARE_FLOAT64("straight_line_test", end_x);
35+
DECLARE_FLOAT64("straight_line_test", end_y);
36+
DECLARE_FLOAT64("straight_line_test", robot_id);
2537

2638
namespace strategy {
2739

@@ -42,15 +54,15 @@ class StraightLineTest : public rclcpp::Node {
4254
rclcpp::Subscription<rj_msgs::msg::GameSettings>::SharedPtr game_settings_sub_;
4355
rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;
4456
rclcpp::Subscription<rj_msgs::msg::AliveRobots>::SharedPtr alive_robots_sub_;
45-
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr line_direction_sub_;
57+
rclcpp::Subscription<rj_msgs::msg::LineTest>::SharedPtr line_direction_sub_;
4658

4759
// subscription callbacks
4860
void world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg);
4961
void play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg);
5062
void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg);
5163
void field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg);
5264
void game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg);
53-
void line_direction_callback(const std_msgs::msg::Bool::SharedPtr& msg);
65+
void line_direction_callback(const rj_msgs::msg::LineTest::SharedPtr& msg);
5466

5567
rclcpp::Publisher<AgentStateMsg>::SharedPtr current_state_publisher_;
5668

@@ -101,7 +113,16 @@ class StraightLineTest : public rclcpp::Node {
101113
[[nodiscard]] WorldState* world_state();
102114
WorldState last_world_state_;
103115
mutable std::mutex world_state_mutex_;
104-
bool vertical_ = false;
116+
rj_geometry::Point start_;
117+
rj_geometry::Point end_;
118+
uint8_t target_robot_id_;
119+
120+
// default field dimensions
121+
float width_min_{-3};
122+
float width_max_{3};
123+
float height_min_{0};
124+
float height_max_{9};
125+
bool field_dimensions_set_{false};
105126
}; // class StraightLineTest
106127

107-
} // namespace strategy
128+
} // namespace strategy

‎src/rj_strategy/src/agent/position/line.cpp‎

Lines changed: 25 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -8,98 +8,37 @@ Line::Line(int r_id) : Position{r_id, "Line"} {}
88

99
Line::Line(int r_id, bool vertical) : Position{r_id, "Line"}, vertical_{vertical} {}
1010

11+
Line::Line(int r_id, rj_geometry::Point start, rj_geometry::Point end, uint8_t target_rid)
12+
: Position{r_id, "Line"}, start_{start}, end_{end}, target_rid_{target_rid} {}
13+
1114
std::optional<RobotIntent> Line::derived_get_task(RobotIntent intent) {
15+
if (robot_id_ != target_rid_) {
16+
return intent;
17+
}
18+
19+
// toggles direction if motion complete
1220
if (check_is_done()) {
1321
forward_ = !forward_;
1422
}
1523

16-
if (vertical_) {
17-
if (forward_) {
18-
auto motion_command = planning::MotionCommand{
19-
"path_target",
20-
planning::LinearMotionInstant{
21-
rj_geometry::Point{
22-
field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1,
23-
field_dimensions_.center_field_loc().y() - 2.5 + 5 * 0.75,
24-
},
25-
rj_geometry::Point{0.0, 0.0},
26-
},
27-
planning::FaceTarget(), true};
28-
29-
intent.motion_command = motion_command;
30-
} else {
31-
auto motion_command = planning::MotionCommand{
32-
"path_target",
33-
planning::LinearMotionInstant{
34-
rj_geometry::Point{
35-
field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1,
36-
field_dimensions_.center_field_loc().y() - 4.5 + 5 * 0.75,
37-
},
38-
rj_geometry::Point{0.0, 0.0},
39-
},
40-
planning::FaceTarget(), true};
41-
42-
intent.motion_command = motion_command;
43-
}
24+
if (forward_) {
25+
// move to start
26+
auto motion_command = planning::MotionCommand{"path_target",
27+
planning::LinearMotionInstant{
28+
start_,
29+
rj_geometry::Point{0.0, 0.0},
30+
},
31+
planning::FaceTarget(), true};
32+
intent.motion_command = motion_command;
4433
} else {
45-
if (forward_) {
46-
if (face_target_) {
47-
auto motion_command = planning::MotionCommand{
48-
"path_target",
49-
planning::LinearMotionInstant{
50-
rj_geometry::Point{
51-
field_dimensions_.center_field_loc().x() - 2.5 + 5 * 0.75,
52-
(field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
53-
},
54-
rj_geometry::Point{0.0, 0.0},
55-
},
56-
planning::FaceTarget(), true};
57-
58-
intent.motion_command = motion_command;
59-
} else {
60-
auto motion_command = planning::MotionCommand{
61-
"path_target",
62-
planning::LinearMotionInstant{
63-
rj_geometry::Point{
64-
field_dimensions_.our_defense_area().maxx(),
65-
(field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
66-
},
67-
rj_geometry::Point{0.0, 0.0},
68-
},
69-
planning::FaceAngle{0}, true};
70-
71-
intent.motion_command = motion_command;
72-
}
73-
74-
} else {
75-
if (face_target_) {
76-
auto motion_command = planning::MotionCommand{
77-
"path_target",
78-
planning::LinearMotionInstant{
79-
rj_geometry::Point{
80-
field_dimensions_.our_defense_area().minx(),
81-
(field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
82-
},
83-
rj_geometry::Point{0.0, 0.0},
84-
},
85-
planning::FaceTarget(), true};
86-
87-
intent.motion_command = motion_command;
88-
} else {
89-
auto motion_command = planning::MotionCommand{
90-
"path_target",
91-
planning::LinearMotionInstant{
92-
rj_geometry::Point{
93-
field_dimensions_.our_defense_area().minx(),
94-
(field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
95-
},
96-
rj_geometry::Point{0.0, 0.0},
97-
},
98-
planning::FaceAngle{0}, true};
99-
100-
intent.motion_command = motion_command;
101-
}
102-
}
34+
// move to end
35+
auto motion_command = planning::MotionCommand{"path_target",
36+
planning::LinearMotionInstant{
37+
end_,
38+
rj_geometry::Point{0.0, 0.0},
39+
},
40+
planning::FaceTarget(), true};
41+
intent.motion_command = motion_command;
10342
}
10443

10544
return intent;

‎src/rj_strategy/src/testing/straight_line_test.cpp‎

Lines changed: 55 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,25 @@
11
#include "rj_strategy/testing/straight_line_test.hpp"
22

3-
namespace strategy {
3+
DEFINE_FLOAT64("straight_line_test", start_x, 1.0, "starting point x coordinate");
4+
DEFINE_FLOAT64("straight_line_test", start_y, 7.0, "starting point y coordinate");
5+
DEFINE_FLOAT64("straight_line_test", end_x, -1.0, "ending point x coordinate");
6+
DEFINE_FLOAT64("straight_line_test", end_y, 7.0, "ending point y coordinate");
7+
DEFINE_FLOAT64("straight_line_test", robot_id, 1, "robot_id for line_test");
8+
9+
namespace strategy { // put code inside of namespace for extra uses
10+
411
using RobotMove = rj_msgs::action::RobotMove;
512
using GoalHandleRobotMove = rclcpp_action::ClientGoalHandle<RobotMove>;
613

7-
StraightLineTest::StraightLineTest() : StraightLineTest(0) {}
14+
StraightLineTest::StraightLineTest() : StraightLineTest(0) {} // creates test for robot 0
815

916
StraightLineTest::StraightLineTest(int r_id)
1017
: rclcpp::Node(::fmt::format("agent_{}_straight_line_node", r_id),
1118
rclcpp::NodeOptions{}
1219
.automatically_declare_parameters_from_overrides(true)
1320
.allow_undeclared_parameters(true)),
14-
current_position_{std::make_unique<Line>(r_id)},
21+
current_position_{std::make_unique<Line>(
22+
r_id)}, // uniq ptr for curr_pos (line, which is child of position)
1523
robot_id_{r_id} {
1624
client_ptr_ = rclcpp_action::create_client<RobotMove>(this, "robot_move");
1725

@@ -40,13 +48,22 @@ StraightLineTest::StraightLineTest(int r_id)
4048
"config/game_settings", 1,
4149
[this](const rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); });
4250

43-
line_direction_sub_ = create_subscription<std_msgs::msg::Bool>(
44-
"line_direction", 1,
45-
[this](const std_msgs::msg::Bool::SharedPtr msg) { line_direction_callback(msg); });
51+
line_direction_sub_ = create_subscription<rj_msgs::msg::LineTest>(
52+
"line", 1,
53+
[this](const rj_msgs::msg::LineTest::SharedPtr msg) { line_direction_callback(msg); });
4654

4755
int hz = 10;
4856
get_task_timer_ = create_wall_timer(std::chrono::milliseconds(1000 / hz),
4957
std::bind(&StraightLineTest::get_task, this));
58+
59+
// sets defaults from yaml config
60+
start_ =
61+
rj_geometry::Point(static_cast<double>(PARAM_start_x), static_cast<double>(PARAM_start_y));
62+
end_ = rj_geometry::Point(static_cast<double>(PARAM_end_x), static_cast<double>(PARAM_end_y));
63+
target_robot_id_ = static_cast<uint8_t>(PARAM_robot_id);
64+
65+
// initiates default
66+
current_position_ = std::make_unique<Line>(robot_id_, start_, end_, target_robot_id_);
5067
}
5168

5269
void StraightLineTest::world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg) {
@@ -66,6 +83,17 @@ void StraightLineTest::field_dimensions_callback(
6683
FieldDimensions field_dimensions = rj_convert::convert_from_ros(*msg);
6784
field_dimensions_ = field_dimensions;
6885
current_position_->update_field_dimensions(field_dimensions);
86+
87+
if (field_dimensions_set_) {
88+
return;
89+
}
90+
91+
width_max_ = field_dimensions_.width() / 2.0f;
92+
width_min_ = -width_max_;
93+
94+
height_max_ = field_dimensions_.length();
95+
96+
field_dimensions_set_ = true;
6997
}
7098

7199
void StraightLineTest::alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg) {
@@ -77,10 +105,26 @@ void StraightLineTest::game_settings_callback(const rj_msgs::msg::GameSettings::
77105
is_simulated_ = msg->simulation;
78106
}
79107

80-
void StraightLineTest::line_direction_callback(const std_msgs::msg::Bool::SharedPtr& msg) {
81-
if (msg->data != vertical_) {
82-
vertical_ = msg->data;
83-
current_position_ = std::make_unique<Line>(robot_id_, vertical_);
108+
// listens for user publish of new movement command
109+
void StraightLineTest::line_direction_callback(const rj_msgs::msg::LineTest::SharedPtr& msg) {
110+
rj_geometry::Point start{msg->pt[0].x, msg->pt[0].y};
111+
rj_geometry::Point end{msg->pt[1].x, msg->pt[1].y};
112+
uint8_t r_id{msg->r_id};
113+
114+
if (start[0] <= width_min_ || end[0] <= width_min_ || start[0] >= width_max_ ||
115+
end[0] >= width_max_ || start[1] <= height_min_ || end[1] <= height_min_ ||
116+
start[1] >= height_max_ || end[1] >= height_max_) {
117+
SPDLOG_INFO("Point Locations Off the FieldDimensions");
118+
return;
119+
}
120+
121+
if (start[0] != start_[0] || start[1] != start_[1] || end[0] != end_[0] || end[1] != end_[1] ||
122+
r_id != target_robot_id_) {
123+
start_ = start;
124+
end_ = end;
125+
target_robot_id_ = r_id;
126+
127+
current_position_ = std::make_unique<Line>(robot_id_, start_, end_, target_robot_id_);
84128
}
85129
}
86130

@@ -187,4 +231,4 @@ int main(int argc, char** argv) {
187231
executor.add_node(agent);
188232
}
189233
executor.spin();
190-
}
234+
}

0 commit comments

Comments
 (0)