Skip to content

Commit

Permalink
Merge pull request #339 from ibis-ssl/feature/advance_ball
Browse files Browse the repository at this point in the history 
ボール先回り
  • Loading branch information
@HansRobo
HansRobo authored May 1, 2024
2 parents ff2a13b + 36a8c2e commit 7011e69
Show file tree
Hide file tree
Showing 8 changed files with 216 additions and 92 deletions.
12 changes: 6 additions & 6 deletions crane_bringup/launch/crane.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,14 @@
<group if="$(var sim)">
<node pkg="crane_local_planner" exec="crane_local_planner_node" output="screen">
<param name="planner" value="gridmap"/>
<param name="p_gain" value="4.0"/>
<param name="p_gain" value="3.0"/>
<param name="i_gain" value="0.00"/>
<param name="i_saturation" value="0.00"/>
<param name="d_gain" value="1.0"/>
<param name="max_vel" value="$(var max_vel)"/>
</node>
<node pkg="crane_sender" exec="sim_sender_node" output="screen">
<param name="no_movement" value="false"/>
<param name="no_movement" value="true"/>
<param name="theta_kp" value="4.5"/>
<param name="theta_ki" value="0.0"/>
<param name="theta_kd" value="0.0"/>
Expand All @@ -42,14 +42,14 @@
<group unless="$(var sim)">
<node pkg="crane_local_planner" exec="crane_local_planner_node" output="screen">
<param name="planner" value="gridmap"/>
<param name="p_gain" value="3.5"/>
<param name="p_gain" value="3.0"/>
<param name="i_gain" value="0.0"/>
<param name="i_saturation" value="0.0"/>
<param name="d_gain" value="0.0"/>
<param name="d_gain" value="0.8"/>
<param name="max_vel" value="$(var max_vel)"/>
</node>
<node pkg="crane_sender" exec="ibis_sender_node" output="screen">
<param name="no_movement" value="false"/>
<param name="no_movement" value="true"/>
<param name="theta_kp" value="5.5"/>
<param name="theta_ki" value="0.0"/>
<param name="theta_kd" value="0.1"/>
Expand Down Expand Up @@ -94,7 +94,7 @@
<param name="plugin_name" value="voicevox_plugin::VoiceVoxPlugin"/>
<param name="voicevox_plugin/speaker" value="14"/>
<param name="voicevox_plugin/speedScale" value="1.0"/>
<param name="voicevox_plugin/volumeScale" value="8.0"/>
<param name="voicevox_plugin/volumeScale" value="10.0"/>
</node>

<node pkg="consai_visualizer" exec="consai_visualizer"/>
Expand Down
41 changes: 38 additions & 3 deletions crane_local_planner/include/crane_local_planner/gridmap_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
#include <crane_msg_wrappers/world_model_wrapper.hpp>
#include <crane_msgs/msg/robot_commands.hpp>
#include <functional>
#include <grid_map_ros/grid_map_ros.hpp>
#include <memory>
#include <nav_msgs/msg/path.hpp>
Expand Down Expand Up @@ -62,6 +63,35 @@ struct AStarNode
bool operator<(const AStarNode & other) const { return getScore() < other.getScore(); }
};

struct ParameterWithEvent
{
ParameterWithEvent(std::string name, rclcpp::Node & node) : name(name)
{
parameter_subscriber = std::make_shared<rclcpp::ParameterEventHandler>(&node);
parameter_callback_handle =
parameter_subscriber->add_parameter_callback(name, [&](const rclcpp::Parameter & p) {
if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
value = p.as_double();
callback(value);
} else {
std::cout << "debug_id is not integer" << std::endl;
}
});
}

std::shared_ptr<rclcpp::ParameterEventHandler> parameter_subscriber;

std::shared_ptr<rclcpp::ParameterCallbackHandle> parameter_callback_handle;

std::function<void(double)> callback;

double getValue() { return value; }

double value;

std::string name;
};

class GridMapPlanner
{
public:
Expand Down Expand Up @@ -89,10 +119,15 @@ class GridMapPlanner
std::array<PIDController, 20> vy_controllers;

double MAX_VEL = 4.0;
double P_GAIN = 4.0;
double I_GAIN = 0.0;

ParameterWithEvent p_gain;
ParameterWithEvent i_gain;
ParameterWithEvent d_gain;

// double P_GAIN = 4.0;
// double I_GAIN = 0.0;
double I_SATURATION = 0.0;
double D_GAIN = 0.0;
// double D_GAIN = 0.0;
};
} // namespace crane
#endif // CRANE_LOCAL_PLANNER__GRIDMAP_PLANNER_HPP_
59 changes: 51 additions & 8 deletions crane_local_planner/src/gridmap_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,28 +13,71 @@ constexpr static int debug_id = -1;
namespace crane
{
GridMapPlanner::GridMapPlanner(rclcpp::Node & node)
: p_gain("p_gain", node), i_gain("i_gain", node), d_gain("d_gain", node)
{
node.declare_parameter("p_gain", 4.0);
p_gain.value = node.get_parameter("p_gain").as_double();
node.declare_parameter("i_gain", 0.0);
i_gain.value = node.get_parameter("i_gain").as_double();
node.declare_parameter("d_gain", 0.0);
d_gain.value = node.get_parameter("d_gain").as_double();

p_gain.callback = [&](double value) {
for (auto & controller : vx_controllers) {
controller.setGain(value, i_gain.getValue(), d_gain.getValue());
}
for (auto & controller : vy_controllers) {
controller.setGain(value, i_gain.getValue(), d_gain.getValue());
}
};

p_gain.callback = [&](double value) {
for (auto & controller : vx_controllers) {
controller.setGain(value, i_gain.getValue(), d_gain.getValue());
}
for (auto & controller : vy_controllers) {
controller.setGain(value, i_gain.getValue(), d_gain.getValue());
}
};

i_gain.callback = [&](double value) {
for (auto & controller : vx_controllers) {
controller.setGain(p_gain.getValue(), value, d_gain.getValue());
}
for (auto & controller : vy_controllers) {
controller.setGain(p_gain.getValue(), value, d_gain.getValue());
}
};

d_gain.callback = [&](double value) {
for (auto & controller : vx_controllers) {
controller.setGain(p_gain.getValue(), i_gain.getValue(), value);
}
for (auto & controller : vy_controllers) {
controller.setGain(p_gain.getValue(), i_gain.getValue(), value);
}
};
node.declare_parameter("map_resolution", MAP_RESOLUTION);
MAP_RESOLUTION = node.get_parameter("map_resolution").as_double();

node.declare_parameter("max_vel", MAX_VEL);
MAX_VEL = node.get_parameter("max_vel").as_double();

node.declare_parameter("p_gain", P_GAIN);
P_GAIN = node.get_parameter("p_gain").as_double();
node.declare_parameter("i_gain", I_GAIN);
I_GAIN = node.get_parameter("i_gain").as_double();
// node.declare_parameter("p_gain", P_GAIN);
// P_GAIN = node.get_parameter("p_gain").as_double();
// node.declare_parameter("i_gain", I_GAIN);
// I_GAIN = node.get_parameter("i_gain").as_double();
node.declare_parameter("i_saturation", I_SATURATION);
I_SATURATION = node.get_parameter("i_saturation").as_double();
node.declare_parameter("d_gain", D_GAIN);
D_GAIN = node.get_parameter("d_gain").as_double();
// node.declare_parameter("d_gain", D_GAIN);
// D_GAIN = node.get_parameter("d_gain").as_double();

for (auto & controller : vx_controllers) {
controller.setGain(P_GAIN, I_GAIN, D_GAIN);
controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION);
}

for (auto & controller : vy_controllers) {
controller.setGain(P_GAIN, I_GAIN, D_GAIN, I_SATURATION);
controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION);
}

visualizer = std::make_shared<ConsaiVisualizerWrapper>(node, "gridmap_local_planner");
Expand Down
44 changes: 35 additions & 9 deletions crane_robot_skills/include/crane_robot_skills/steal_ball.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <crane_robot_skills/skill_base.hpp>
#include <memory>
#include <string>
#include <vector>

namespace crane::skills
{
Expand Down Expand Up @@ -84,7 +85,7 @@ class StealBall : public SkillBase<StealBallState>
command->dribble(0.5);
} else if (method == "side") {
command->setDribblerTargetPosition(world_model->ball.pos);
if (robot->getDistance(world_model->ball.pos) < (0.085 + 0.005)) {
if (robot->getDistance(world_model->ball.pos) < (0.085 + 0.000)) {
// ロボット半径より近くに来れば急回転して刈り取れる
command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos) + M_PI / 2);
} else {
Expand Down Expand Up @@ -156,22 +157,47 @@ class StealBall : public SkillBase<StealBallState>
});

addStateFunction(StealBallState::INTERCEPT, [this](const ConsaiVisualizerWrapper::SharedPtr &) {
Segment ball_line{
world_model->ball.pos, world_model->ball.pos + world_model->ball.vel.normalized() * 10.0};
Point vel_seg = world_model->ball.vel * 5.0;
if (vel_seg.norm() < 1.0) {
vel_seg = vel_seg.normalized() * 1.0;
}
Segment ball_line{world_model->ball.pos, world_model->ball.pos + vel_seg};

Point across_point = [&]() {
const double OFFSET = 0.3;
const double X = world_model->field_size.x() / 2.0 - OFFSET;
const double Y = world_model->field_size.y() / 2.0 - OFFSET;

Segment seg1{Point(X, Y), Point(X, -Y)};
Segment seg2{Point(-X, Y), Point(-X, -Y)};
Segment seg3{Point(Y, X), Point(-Y, X)};
Segment seg4{Point(Y, -X), Point(-Y, -X)};
std::vector<Point> intersections;
if (bg::intersection(ball_line, seg1, intersections); not intersections.empty()) {
return intersections.front();
} else if (bg::intersection(ball_line, seg2, intersections); not intersections.empty()) {
return intersections.front();
} else if (bg::intersection(ball_line, seg3, intersections); not intersections.empty()) {
return intersections.front();
} else if (bg::intersection(ball_line, seg4, intersections); not intersections.empty()) {
return intersections.front();
} else {
return ball_line.second;
}
}();

ClosestPoint result;
bg::closest_point(robot->pose.pos, ball_line, result);
// ClosestPoint result;
// bg::closest_point(robot->pose.pos, ball_line, result);

// ゴールとボールの中間方向を向く
auto [goal_angle, width] =
world_model->getLargestGoalAngleRangeFromPoint(result.closest_point);
auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(across_point);
auto to_goal = getNormVec(goal_angle);
auto to_ball = (world_model->ball.pos - result.closest_point).normalized();
auto to_ball = (world_model->ball.pos - across_point).normalized();
double intermediate_angle = getAngle(2 * to_goal + to_ball);
command->setTargetTheta(intermediate_angle);
command->liftUpDribbler();
command->kickStraight(getParameter<double>("kicker_power"));
command->setDribblerTargetPosition(result.closest_point);
command->setDribblerTargetPosition(across_point);

return Status::RUNNING;
});
Expand Down
4 changes: 2 additions & 2 deletions crane_robot_skills/src/single_ball_placement.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr<World
[this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
command->setDribblerTargetPosition(pull_back_target.value());
// 角度はそのまま引っ張りたいので指定はしない
command->dribble(0.3);
command->dribble(0.2);
command->setMaxVelocity(0.3);
command->disablePlacementAvoidance();
command->disableGoalAreaAvoidance();
Expand Down Expand Up @@ -208,7 +208,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr<World
move_with_ball->setCommander(command);
move_with_ball->setParameter("target_x", getParameter<double>("placement_x"));
move_with_ball->setParameter("target_y", getParameter<double>("placement_y"));
move_with_ball->setParameter("dribble_power", 0.3);
move_with_ball->setParameter("dribble_power", 0.2);
}

skill_status = move_with_ball->run(visualizer);
Expand Down
4 changes: 4 additions & 0 deletions session/crane_planner_plugins/include/crane_planner_plugins/our_free_kick_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@ class OurDirectFreeKickPlanner : public PlannerBase

std::vector<std::shared_ptr<RobotCommandWrapper>> other_robots;

bool fake_over = false;

int fake_count = 0;

public:
COMPOSITION_PUBLIC
explicit OurDirectFreeKickPlanner(
Expand Down
Loading

0 comments on commit 7011e69

Please sign in to comment.