Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

MFT2024変更監視 #563

Draft
wants to merge 13 commits into
base: develop
Choose a base branch
from
+386 −201
Draft

MFT2024変更監視 #563

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
267a5ba
control_modeのデフォルトを位置モードに
HansRobo Sep 21, 2024
611ca4c
GoalKickクラスの回り込み距離を変更
HansRobo Sep 21, 2024
96de4cf
キーパーの待機距離を変更
HansRobo Sep 21, 2024
40db400
senderが落ちるバグをい修正
HansRobo Sep 21, 2024
3541ed0
MFT用のlaunchファイルを追加
HansRobo Sep 21, 2024
dd08f8f
dribble時に強制的にキックパワーのリセットをしないようにする
HansRobo Sep 21, 2024
34b8744
狭いのでルール無視
HansRobo Sep 21, 2024
39a2af3
teleop改良
HansRobo Sep 21, 2024
d536fad
pre-commit
HansRobo Sep 21, 2024
569bbd9
Merge remote-tracking branch 'origin/develop' into mft2024
HansRobo Oct 3, 2024
4c2f816
teleop改良
HansRobo Oct 11, 2024
e6ea1a4
未コミット
HansRobo Oct 11, 2024
70cd5ca
style(pre-commit): autofix
pre-commit-ci[bot] Oct 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
142 changes: 142 additions & 0 deletions crane_bringup/launch/mft.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,142 @@
<launch>
<!-- Launch Arguments -->
<arg name="vision_addr" default="224.5.23.2" description="Set multicast address to connect SSL-Vision."/>
<arg name="vision_port" default="10006" description="Set multicast port to connect SSL-Vision."/>
<arg name="referee_addr" default="224.5.23.1" description="Set multicast address to connect Game Controller."/>
<!-- <arg name="referee_port" default="11111" description="Set multicast port to connect Game Controller."/>-->
<arg name="referee_port" default="10003" description="Set multicast port to connect Game Controller."/>
<arg name="team" default="KIKS" description="team name"/>
<arg name="sim" default="false" description="Set true if you want to use simulator."/>
<arg name="original_grsim" default="false" description="Set true if you want to default grsim."/>
<!-- <arg name="simple_ai" default="true" description="a"/>-->
<arg name="simple_ai" default="false" description="a"/>
<arg name="max_vel" default="1.5" description="Set max velocity of robot."/>
<arg name="gui" default="true" description="Set true if you want to use GUI."/>
<arg name="speak" default="true" description="Set true if you want to use speaker."/>
<set_parameter name="use_sim_time" value="$(var sim)"/>

<!-- Nodes -->
<group if="$(var simple_ai)">
<node pkg="crane_simple_ai" exec="crane_simple_ai" output="screen"/>
</group>

<group unless="$(var simple_ai)">
<node pkg="crane_session_controller" exec="crane_session_controller_node" output="screen">
<param name="initial_session" value="STOP"/>
<param name="event_config_file_name" value="normal.yaml"/>
</node>
</group>

<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="2.0"/>
<param name="i_gain" value="0.00"/>
<param name="i_saturation" value="0.00"/>
<param name="d_gain" value="3.0"/>
<param name="max_vel" value="$(var max_vel)"/>
</node>

<node pkg="crane_clock_publisher" exec="crane_clock_publisher_node" output="screen">
<param name="time_scale" value="1.00"/>
</node>
</group>

<node pkg="joy" exec="joy_node" output="screen"/>

<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.0"/>
<param name="i_gain" value="0.0"/>
<param name="i_saturation" value="0.0"/>
<param name="d_gain" value="1.5"/>
<param name="max_vel" value="$(var max_vel)"/>
</node>
</group>

<group if="$(var original_grsim)">
<node pkg="crane_sender" exec="sim_sender_node" output="screen">
<param name="no_movement" value="false"/>
<param name="latency_ms" value="0.0"/>
<param name="k_gain" value="1.5"/>
<param name="i_gain" value="0.0"/>
<param name="d_gain" value="1.5"/>
<param name="theta_k_gain" value="2.0"/>
<param name="theta_i_gain" value="0.0"/>
<param name="theta_d_gain" value="0.1"/>
<param name="kick_power_limit_straight" value="0.3"/>
<param name="kick_power_limit_chip" value="0.5"/>
<param name="sim_mode" value="true"/>
</node>
</group>

<group unless="$(var original_grsim)">
<node pkg="crane_sender" exec="ibis_sender_node">
<param name="no_movement" value="false"/>
<param name="latency_ms" value="0.0"/>
<param name="theta_kp" value="3.5"/>
<param name="theta_ki" value="0.0"/>
<param name="theta_kd" value="0.5"/>
<param name="sim_mode" value="$(var sim)"/>
<param name="kick_power_limit_straight" value="0.4"/>
<param name="kick_power_limit_chip" value="0.4"/>
</node>

<!-- <node pkg="crane_sender" exec="ibis_velocity_sender_node">-->
<!-- <param name="no_movement" value="false"/>-->
<!-- <param name="latency_ms" value="0.0"/>-->
<!-- <param name="k_gain" value="1.0"/>-->
<!-- <param name="i_gain" value="0.0"/>-->
<!-- <param name="d_gain" value="1.5"/>-->
<!-- <param name="sim_mode" value="$(var sim)"/>-->
<!-- <param name="kick_power_limit_straight" value="0.3"/>-->
<!-- <param name="kick_power_limit_chip" value="0.3"/>-->
<!-- </node>-->
</group>

<node pkg="robocup_ssl_comm" exec="vision_node">
<param name="multicast_address" value="$(var vision_addr)"/>
<param name="multicast_port" value="$(var vision_port)"/>
</node>

<node pkg="robocup_ssl_comm" exec="game_controller_node">
<param name="multicast_address" value="$(var referee_addr)"/>
<param name="multicast_port" value="$(var referee_port)"/>
</node>

<node pkg="robocup_ssl_comm" exec="grsim_node"/>

<node pkg="crane_robot_receiver" exec="robot_receiver_node" output="screen"/>

<node pkg="robocup_ssl_comm" exec="robot_status_node">
<param name="blue_port" value="10311"/>
<param name="yellow_port" value="10312"/>
</node>

<node pkg="consai_vision_tracker" exec="vision_tracker_node"/>

<node pkg="crane_world_model_publisher" exec="crane_world_model_publisher_node">
<param name="initial_team_color" value="YELLOW"/>
<param name="team_name" value="$(var team)"/>
</node>

<node pkg="crane_play_switcher" exec="play_switcher_node" output="screen">
<param name="team_name" value="$(var team)"/>
</node>

<group if="$(var speak)">
<node pkg="crane_speaker" exec="crane_speaker_node"/>
</group>

<node pkg="speak_ros" exec="speak_ros_node" output="screen">
<param name="plugin_name" value="voicevox_plugin::VoiceVoxPlugin"/>
<param name="voicevox_plugin/speaker" value="0"/>
<param name="voicevox_plugin/speedScale" value="1.0"/>
<param name="voicevox_plugin/volumeScale" value="14.0"/>
</node>

<group if="$(var gui)">
<node pkg="consai_visualizer" exec="consai_visualizer"/>
</group>
</launch>
13 changes: 7 additions & 6 deletions crane_local_planner/src/gridmap_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -199,9 +199,10 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
case crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE:
case crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE:
// ほんとうは0.2mだがバッファを0.2mとる
return 0.2 + 0.2;
// return 0.2 + 0.2;
return 0.0;
default:
return 0.1;
return 0.0;
}
}();

Expand Down Expand Up @@ -236,7 +237,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
}
map["friend_robot"].setZero();
for (const auto & robot : world_model->ours.getAvailableRobots()) {
for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.2); !iterator.isPastEnd();
for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd();
++iterator) {
map.at("friend_robot", *iterator) = 1.0;
}
Expand All @@ -248,7 +249,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
}
map["enemy_robot"].setZero();
for (const auto & robot : world_model->theirs.getAvailableRobots()) {
for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.2); !iterator.isPastEnd();
for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd();
++iterator) {
map.at("enemy_robot", *iterator) = 1.0;
}
Expand All @@ -272,7 +273,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
switch (world_model->play_situation.getSituationCommandID()) {
case crane_msgs::msg::PlaySituation::STOP: {
// 5.1.1 ボールと0.5m以上離れる必要がある
for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.6);
for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.2);
!iterator.isPastEnd(); ++iterator) {
map.at("rule", *iterator) = 1.0;
}
Expand All @@ -282,7 +283,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
grid_map::Position position;
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
map.getPosition(*iterator, position);
map.at("rule", *iterator) = world_model->point_checker.isBallPlacementArea(position, 0.2);
map.at("rule", *iterator) = world_model->point_checker.isBallPlacementArea(position, -0.2);
}
} break;
// case crane_msgs::msg::PlaySituation::THEIR_KICKOFF_PREPARATION:
Expand Down
2 changes: 1 addition & 1 deletion crane_msgs/msg/control/RobotCommand.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ uint8 POSITION_TARGET_MODE=1
uint8 SIMPLE_VELOCITY_TARGET_MODE=2
uint8 VELOCITY_TARGET_MODE=3

uint8 control_mode
uint8 control_mode 1

string skill_name

Expand Down
1 change: 1 addition & 0 deletions crane_robot_skills/include/crane_robot_skills/goal_kick.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ class GoalKick : public SkillBase<RobotCommandWrapperPosition>
kick_skill.setParameter("chip_kick", false);
kick_skill.setParameter("with_dribble", false);
kick_skill.setParameter("dot_threshold", getParameter<double>("dot_threshold"));
kick_skill.setParameter("around_interval", 0.25f);
}

Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
Expand Down
156 changes: 2 additions & 154 deletions crane_robot_skills/include/crane_robot_skills/teleop.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,161 +18,9 @@ namespace crane::skills
class Teleop : public SkillBase<RobotCommandWrapperPosition>, public rclcpp::Node
{
public:
explicit Teleop(RobotCommandWrapperBase::SharedPtr & base)
: SkillBase("Teleop", base), Node("teleop_skill")
{
setParameter("rotation_deg", 0.);
setParameter("use_local_coordinate", false);
std::cout << "Teleop skill created" << std::endl;
joystick_subscription = this->create_subscription<sensor_msgs::msg::Joy>(
"/joy", 10, [this](const sensor_msgs::msg::Joy & msg) {
std::cout << "joy message received" << std::endl;
last_joy_msg = msg;
});
std::cout << "joy subscriber created" << std::endl;
}
explicit Teleop(RobotCommandWrapperBase::SharedPtr & base);

Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
{
rclcpp::spin_some(this->get_node_base_interface());
if (last_joy_msg.buttons.empty()) {
std::cout << "no joy message" << std::endl;
return Status::RUNNING;
}

const int BUTTON_POWER_ENABLE = 9;

const int AXIS_VEL_SURGE = 1;
const int AXIS_VEL_SWAY = 0;
const int AXIS_VEL_ANGULAR_R = 2;
const int AXIS_VEL_ANGULAR_L = 5;

const int BUTTON_KICK_TOGGLE = 4;
const int BUTTON_KICK_STRAIGHT = 13;
const int BUTTON_KICK_CHIP = 14;
const int BUTTON_ADJUST_KICK = 0;

const int BUTTON_ADJUST = 10;
const int BUTTON_ADJUST_UP = 11;
const int BUTTON_ADJUST_DOWN = 12;

const int BUTTON_DRIBBLE_TOGGLE = 6;
const int BUTTON_ADJUST_DRIBBLE = 1;

const double MAX_VEL_SURGE = 1.0;
const double MAX_VEL_SWAY = 1.0;
const double MAX_VEL_ANGULAR = M_PI * 0.1;

static bool is_kick_mode_straight = true;
static bool is_kick_enable = false;
static bool is_dribble_enable = false;

if (last_joy_msg.buttons[BUTTON_KICK_CHIP]) {
std::cout << "chip mode" << std::endl;
is_kick_mode_straight = false;
}
if (last_joy_msg.buttons[BUTTON_KICK_STRAIGHT]) {
std::cout << "straight mode" << std::endl;
is_kick_mode_straight = true;
}

auto update_mode = [&](bool & mode_variable, const int button, bool & is_pushed) {
// trigger button up
if (last_joy_msg.buttons[button]) {
if (!is_pushed) {
std::cout << "toggle mode!" << std::endl;
mode_variable = not mode_variable;
}
is_pushed = true;
} else {
is_pushed = false;
}
};

static bool is_pushed_kick = false;
static bool is_pushed_dribble = false;

update_mode(is_kick_enable, BUTTON_KICK_TOGGLE, is_pushed_kick);
update_mode(is_dribble_enable, BUTTON_DRIBBLE_TOGGLE, is_pushed_dribble);

auto adjust_value = [](double & value, const double step) {
value += step;
value = std::clamp(value, 0.0, 1.0);
};

if (last_joy_msg.buttons[BUTTON_ADJUST]) {
static bool is_pushed = false;
if (last_joy_msg.buttons[BUTTON_ADJUST_UP]) {
// trigger button up
if (!is_pushed) {
if (last_joy_msg.buttons[BUTTON_ADJUST_KICK]) {
adjust_value(kick_power, 0.1);
std::cout << "kick up: " << kick_power << std::endl;
}

if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) {
adjust_value(dribble_power, 0.1);
std::cout << "dribble up:" << dribble_power << std::endl;
}
}
is_pushed = true;
} else if (last_joy_msg.buttons[BUTTON_ADJUST_DOWN]) {
// trigger button up
if (!is_pushed) {
if (last_joy_msg.buttons[BUTTON_ADJUST_KICK]) {
adjust_value(kick_power, -0.1);
std::cout << "kick down: " << kick_power << std::endl;
}
if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) {
adjust_value(dribble_power, -0.1);
std::cout << "dribble down: " << dribble_power << std::endl;
}
}
is_pushed = true;
} else {
is_pushed = false;
}
}

Point target = [&]() -> Point {
using boost::math::constants::degree;
double rotation_angle = getParameter<double>("rotation_deg") * degree<double>();
if (getParameter<bool>("use_local_coordinate")) {
rotation_angle += robot()->pose.theta;
}
Eigen::Rotation2Dd rotation(rotation_angle);
return robot()->pose.pos +
rotation.toRotationMatrix() * Point{
last_joy_msg.axes[AXIS_VEL_SURGE] * MAX_VEL_SURGE,
last_joy_msg.axes[AXIS_VEL_SWAY] * MAX_VEL_SWAY};
}();

command.setTargetPosition(target);

double angular =
(1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_R]) - (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_L]);

theta += angular;
command.setTargetTheta(normalizeAngle(theta));

if (is_kick_enable) {
if (is_kick_mode_straight) {
command.kickStraight(kick_power);
} else {
command.kickWithChip(kick_power);
}
} else {
command.kickStraight(0.0);
}

if (is_dribble_enable) {
command.dribble(dribble_power);
} else {
command.dribble(0.0);
}

return Status::RUNNING;
}
Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;

void print(std::ostream & os) const override { os << "[Teleop]"; }

Expand Down
2 changes: 1 addition & 1 deletion crane_robot_skills/src/goalie.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ Goalie::Goalie(RobotCommandWrapperBase::SharedPtr & base)
kick_skill(base)
{
setParameter("run_inplay", true);
setParameter("block_distance", 1.0);
setParameter("block_distance", 0.3);
}

Status Goalie::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer)
Expand Down
Loading