From 8c4524b9d3cbbc53c888a6d4f6c56e118f536af4 Mon Sep 17 00:00:00 2001 From: Mirai Hattori Date: Tue, 10 Dec 2019 13:18:30 +0900 Subject: [PATCH 01/13] use boost asio in consai2r2 sender --- consai2r2_sender/CMakeLists.txt | 9 +++++-- consai2r2_sender/package.xml | 5 ++-- consai2r2_sender/src/sim_sender.cpp | 42 +++++++++++++---------------- 3 files changed, 29 insertions(+), 27 deletions(-) diff --git a/consai2r2_sender/CMakeLists.txt b/consai2r2_sender/CMakeLists.txt index 28788f8..b211072 100644 --- a/consai2r2_sender/CMakeLists.txt +++ b/consai2r2_sender/CMakeLists.txt @@ -16,9 +16,14 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(consai2r2_msgs REQUIRED) find_package(Protobuf REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) # Protobuf -include_directories(${PROTOBUF_INCLUDE_DIRS}) +include_directories( + ${PROTOBUF_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + protobuf_generate_cpp(PROTO_CPP PROTO_H include/${PROJECT_NAME}/proto/grSim_Replacement.proto include/${PROJECT_NAME}/proto/grSim_Commands.proto @@ -37,7 +42,7 @@ ament_target_dependencies( "std_msgs" "consai2r2_msgs" ) -target_link_libraries(sim_sender ${PROTOBUF_LIBRARIES}) +target_link_libraries(sim_sender ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS sim_sender diff --git a/consai2r2_sender/package.xml b/consai2r2_sender/package.xml index a342079..bf2df8d 100644 --- a/consai2r2_sender/package.xml +++ b/consai2r2_sender/package.xml @@ -3,9 +3,9 @@ consai2r2_sender 0.0.0 - TODO: Package description + Send commands to grSim or real robots akshota - TODO: License declaration + MIT ament_cmake @@ -13,6 +13,7 @@ std_msgs consai2r2_msgs protobuf-dev + boost ament_lint_auto ament_lint_common diff --git a/consai2r2_sender/src/sim_sender.cpp b/consai2r2_sender/src/sim_sender.cpp index 49cbf93..2590b09 100644 --- a/consai2r2_sender/src/sim_sender.cpp +++ b/consai2r2_sender/src/sim_sender.cpp @@ -18,14 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -// for UDP communication -#include -#include -#include -#include -#include -#include - +#include #include #include #include @@ -40,25 +33,28 @@ using std::placeholders::_1; -class simple_udp -{ - int sock; - struct sockaddr_in addr; +namespace asio = boost::asio; +class UDPSender +{ public: - simple_udp(std::string address, int port) + UDPSender(const std::string & ip, const int port) + : socket_(io_service_, asio::ip::udp::endpoint(asio::ip::udp::v4(), 0)) { - sock = socket(AF_INET, SOCK_DGRAM, 0); - addr.sin_family = AF_INET; - addr.sin_addr.s_addr = inet_addr(address.c_str()); - addr.sin_port = htons(port); + asio::ip::udp::resolver resolver(io_service_); + asio::ip::udp::resolver::query query(ip, std::to_string(port)); + endpoint_ = *resolver.resolve(query); } - void udp_send(std::string word) + + void send(const std::string & str) { - sendto(sock, word.c_str(), word.length(), 0, (struct sockaddr *)&addr, sizeof(addr)); + socket_.send_to(asio::buffer(str), endpoint_); } - ~simple_udp() {close(sock);} +private: + asio::io_service io_service_; + asio::ip::udp::endpoint endpoint_; + asio::ip::udp::socket socket_; }; class SimSender : public rclcpp::Node @@ -79,7 +75,7 @@ class SimSender : public rclcpp::Node sub_commands_ = this->create_subscription( "robot_commands", 10, std::bind(&SimSender::send_commands, this, std::placeholders::_1)); - udp_ = std::make_shared(host, port); + udp_sender_ = std::make_shared(host, port); } private: @@ -123,11 +119,11 @@ class SimSender : public rclcpp::Node std::string output; packet.SerializeToString(&output); - udp_->udp_send(output); + udp_sender_->send(output); } rclcpp::Subscription::SharedPtr sub_commands_; - std::shared_ptr udp_; + std::shared_ptr udp_sender_; }; int main(int argc, char * argv[]) From d00e2d6f1b87eab7531d394b9c38a7ecc19e8c2f Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Sat, 7 Dec 2019 17:51:41 +0900 Subject: [PATCH 02/13] Impremented RefereeReceiver --- consai2r2_msgs/CMakeLists.txt | 8 +- consai2r2_msgs/msg/Referee.msg | 23 +++ consai2r2_msgs/msg/RefereeGameEvent.msg | 11 ++ consai2r2_msgs/msg/RefereeTeamInfo.msg | 21 +++ consai2r2_msgs/package.xml | 5 +- consai2r2_receiver/CMakeLists.txt | 68 ++++++++ .../consai2r2_receiver/proto/game_event.proto | 100 +++++++++++ .../messages_robocup_ssl_detection.proto | 32 ++++ .../proto/messages_robocup_ssl_geometry.proto | 72 ++++++++ .../messages_robocup_ssl_refbox_log.proto | 14 ++ .../messages_robocup_ssl_robot_status.proto | 12 ++ .../proto/messages_robocup_ssl_wrapper.proto | 8 + .../consai2r2_receiver/proto/referee.proto | 164 ++++++++++++++++++ consai2r2_receiver/include/multicast.hpp | 44 +++++ consai2r2_receiver/package.xml | 24 +++ consai2r2_receiver/src/referee_receiver.cpp | 112 ++++++++++++ 16 files changed, 716 insertions(+), 2 deletions(-) create mode 100755 consai2r2_msgs/msg/Referee.msg create mode 100755 consai2r2_msgs/msg/RefereeGameEvent.msg create mode 100755 consai2r2_msgs/msg/RefereeTeamInfo.msg create mode 100644 consai2r2_receiver/CMakeLists.txt create mode 100755 consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto create mode 100644 consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto create mode 100644 consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto create mode 100644 consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto create mode 100644 consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto create mode 100644 consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto create mode 100755 consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto create mode 100644 consai2r2_receiver/include/multicast.hpp create mode 100644 consai2r2_receiver/package.xml create mode 100644 consai2r2_receiver/src/referee_receiver.cpp diff --git a/consai2r2_msgs/CMakeLists.txt b/consai2r2_msgs/CMakeLists.txt index 882d7b3..817c082 100644 --- a/consai2r2_msgs/CMakeLists.txt +++ b/consai2r2_msgs/CMakeLists.txt @@ -13,16 +13,22 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) set(msg_files + "msg/RefereeGameEvent.msg" + "msg/RefereeTeamInfo.msg" + "msg/Referee.msg" "msg/RobotCommand.msg" "msg/RobotCommands.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} - DEPENDENCIES std_msgs + DEPENDENCIES builtin_interfaces std_msgs geometry_msgs sensor_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/consai2r2_msgs/msg/Referee.msg b/consai2r2_msgs/msg/Referee.msg new file mode 100755 index 0000000..866ea0d --- /dev/null +++ b/consai2r2_msgs/msg/Referee.msg @@ -0,0 +1,23 @@ +# This msg file is copied from 'referee.proto' + +# These are the "coarse" stages of the game. +uint8 stage + +# These are the "fine" states of play on the field. +uint8 command + +# The number of commands issued since startup (mod 2^32). +uint32 command_counter + +# Information about the two teams. +RefereeTeamInfo yellow +RefereeTeamInfo blue + +# The coordinates of the Designated Position. These are measured in +# millimetres and correspond to SSL-Vision coordinates. These fields are +# always either both present (in the case of a ball placement command) or +# both absent (in the case of any other command). +geometry_msgs/Point designated_position + +# The game event that caused the referee command +RefereeGameEvent game_event \ No newline at end of file diff --git a/consai2r2_msgs/msg/RefereeGameEvent.msg b/consai2r2_msgs/msg/RefereeGameEvent.msg new file mode 100755 index 0000000..abdd0f3 --- /dev/null +++ b/consai2r2_msgs/msg/RefereeGameEvent.msg @@ -0,0 +1,11 @@ +# This msg file is copied from 'game_event.proto' + +# the game event type that happened +uint8 game_event_type + +# information about an originator +uint8 originator_team +uint32 originator_bot_id + +# a message describing further details of this game event +string message \ No newline at end of file diff --git a/consai2r2_msgs/msg/RefereeTeamInfo.msg b/consai2r2_msgs/msg/RefereeTeamInfo.msg new file mode 100755 index 0000000..27f9e8b --- /dev/null +++ b/consai2r2_msgs/msg/RefereeTeamInfo.msg @@ -0,0 +1,21 @@ +# This msg file is copied from 'referee.proto' + +# The team's name (empty string if operator has not typed anything). +string name +# The number of goals scored by the team during normal play and overtime. +uint32 score +# The number of red cards issued to the team since the beginning of the game. +uint32 red_cards +# The amount of time (in microseconds) left on each yellow card issued to the team. +# If no yellow cards are issued, this array has no elements. +# Otherwise, times are ordered from smallest to largest. +uint32[] yellow_card_times +# The total number of yellow cards ever issued to the team. +uint32 yellow_cards +# The number of timeouts this team can still call. +# If in a timeout right now, that timeout is excluded. +uint32 timeouts +# The number of microseconds of timeout this team can use. +uint32 timeout_time +# The pattern number of this team's goalie. +uint32 goalie \ No newline at end of file diff --git a/consai2r2_msgs/package.xml b/consai2r2_msgs/package.xml index ae2346f..91fe89c 100644 --- a/consai2r2_msgs/package.xml +++ b/consai2r2_msgs/package.xml @@ -5,12 +5,15 @@ 0.0.0 TODO: Package description akshota - TODO: License declaration + MIT ament_cmake rosidl_default_generators + builtin_interfaces std_msgs + sensor_msgs + geometry_msgs rosidl_interface_packages diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt new file mode 100644 index 0000000..d12f551 --- /dev/null +++ b/consai2r2_receiver/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(consai2r2_receiver) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(consai2r2_msgs REQUIRED) +find_package(Protobuf REQUIRED) +find_package(Boost REQUIRED) + +# Protobuf +include_directories( + "include" + ${PROTOBUF_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +protobuf_generate_cpp(PROTO_CPP PROTO_H + include/${PROJECT_NAME}/proto/messages_robocup_ssl_wrapper.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_geometry.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_detection.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_robot_status.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_refbox_log.proto + include/${PROJECT_NAME}/proto/referee.proto + include/${PROJECT_NAME}/proto/game_event.proto +) + +include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) +add_executable(referee_receiver + src/referee_receiver.cpp + ${PROTO_CPP} + ${PROTO_H} +) +ament_target_dependencies( + referee_receiver + "rclcpp" + "std_msgs" + "consai2r2_msgs" +) +target_link_libraries(referee_receiver ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES}) + + +install(TARGETS referee_receiver + EXPORT export_${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto new file mode 100755 index 0000000..94385aa --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto @@ -0,0 +1,100 @@ +syntax = "proto2"; + +// a game event that caused a referee command +message SSL_Referee_Game_Event { + + enum GameEventType { + // not set + UNKNOWN = 0; + + // an event that is not listed in this enum yet. + // Give further details in the message below + CUSTOM = 1; + + // Law 3: Number of players + NUMBER_OF_PLAYERS = 2; + + // Law 9: Ball out of play + BALL_LEFT_FIELD = 3; + + // Law 10: Team scored a goal + GOAL = 4; + + // Law 9.3: lack of progress while bringing the ball into play + KICK_TIMEOUT = 5; + + // Law ?: There is no progress in game for (10|15)? seconds + NO_PROGRESS_IN_GAME = 6; + + // Law 12: Pushing / Substantial Contact + BOT_COLLISION = 7; + + // Law 12.2: Defender is completely inside penalty area + MULTIPLE_DEFENDER = 8; + + // Law 12: Defender is partially inside penalty area + MULTIPLE_DEFENDER_PARTIALLY = 9; + + // Law 12.3: Attacker in defense area + ATTACKER_IN_DEFENSE_AREA = 10; + + // Law 12: Icing (kicking over midline and opponent goal line) + ICING = 11; + + // Law 12: Ball speed + BALL_SPEED = 12; + + // Law 12: Robot speed during STOP + ROBOT_STOP_SPEED = 13; + + // Law 12: Maximum dribbling distance + BALL_DRIBBLING = 14; + + // Law 12: Touching the opponent goalkeeper + ATTACKER_TOUCH_KEEPER = 15; + + // Law 12: Double touch + DOUBLE_TOUCH = 16; + + // Law 13-17: Attacker not too close to the opponent's penalty area when ball enters play + ATTACKER_TO_DEFENCE_AREA = 17; + + // Law 13-17: Keeping the correct distance to the ball during opponents freekicks + DEFENDER_TO_KICK_POINT_DISTANCE = 18; + + // Law 12: A robot holds the ball deliberately + BALL_HOLDING = 19; + + // Law 12: The ball entered the goal directly after an indirect kick was performed + INDIRECT_GOAL = 20; + + // Law 9.2: Ball placement + BALL_PLACEMENT_FAILED = 21; + + // Law 10: A goal is only scored if the ball has not exceeded a robot height (150mm) between the last + // kick of an attacker and the time the ball crossed the goal line. + CHIP_ON_GOAL = 22; + } + + // the game event type that happened + required GameEventType gameEventType = 1; + + // a team + enum Team { + TEAM_UNKNOWN = 0; + TEAM_YELLOW = 1; + TEAM_BLUE = 2; + } + + // information about an originator + message Originator { + required Team team = 1; + optional uint32 botId = 2; + } + + // the team and optionally a designated robot that is the originator of the game event + optional Originator originator = 2; + + // a message describing further details of this game event + optional string message = 3; +} \ No newline at end of file diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto new file mode 100644 index 0000000..918748c --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto @@ -0,0 +1,32 @@ +syntax = "proto2"; + +message SSL_DetectionBall { + required float confidence = 1; + optional uint32 area = 2; + required float x = 3; + required float y = 4; + optional float z = 5; + required float pixel_x = 6; + required float pixel_y = 7; +} + +message SSL_DetectionRobot { + required float confidence = 1; + optional uint32 robot_id = 2; + required float x = 3; + required float y = 4; + optional float orientation = 5; + required float pixel_x = 6; + required float pixel_y = 7; + optional float height = 8; +} + +message SSL_DetectionFrame { + required uint32 frame_number = 1; + required double t_capture = 2; + required double t_sent = 3; + required uint32 camera_id = 4; + repeated SSL_DetectionBall balls = 5; + repeated SSL_DetectionRobot robots_yellow = 6; + repeated SSL_DetectionRobot robots_blue = 7; +} diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto new file mode 100644 index 0000000..6a0b991 --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto @@ -0,0 +1,72 @@ +syntax = "proto2"; + +// A 2D float vector. +message Vector2f { + required float x = 1; + required float y = 2; +} + +// Represents a field marking as a line segment represented by a start point p1, +// and end point p2, and a line thickness. The start and end points are along +// the center of the line, so the thickness of the line extends by thickness / 2 +// on either side of the line. +message SSL_FieldLineSegment { + // Name of this field marking. + required string name = 1; + // Start point of the line segment. + required Vector2f p1 = 2; + // End point of the line segment. + required Vector2f p2 = 3; + // Thickness of the line segment. + required float thickness = 4; +} + +// Represents a field marking as a circular arc segment represented by center point, a +// start angle, an end angle, and an arc thickness. +message SSL_FieldCicularArc { + // Name of this field marking. + required string name = 1; + // Center point of the circular arc. + required Vector2f center = 2; + // Radius of the arc. + required float radius = 3; + // Start angle in counter-clockwise order. + required float a1 = 4; + // End angle in counter-clockwise order. + required float a2 = 5; + // Thickness of the arc. + required float thickness = 6; +} + +message SSL_GeometryFieldSize { + required int32 field_length = 1; + required int32 field_width = 2; + required int32 goal_width = 3; + required int32 goal_depth = 4; + required int32 boundary_width = 5; + repeated SSL_FieldLineSegment field_lines = 6; + repeated SSL_FieldCicularArc field_arcs = 7; +} + +message SSL_GeometryCameraCalibration { + required uint32 camera_id = 1; + required float focal_length = 2; + required float principal_point_x = 3; + required float principal_point_y = 4; + required float distortion = 5; + required float q0 = 6; + required float q1 = 7; + required float q2 = 8; + required float q3 = 9; + required float tx = 10; + required float ty = 11; + required float tz = 12; + optional float derived_camera_world_tx = 13; + optional float derived_camera_world_ty = 14; + optional float derived_camera_world_tz = 15; +} + +message SSL_GeometryData { + required SSL_GeometryFieldSize field = 1; + repeated SSL_GeometryCameraCalibration calib = 2; +} diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto new file mode 100644 index 0000000..8a437bc --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto @@ -0,0 +1,14 @@ +syntax = "proto2"; + +import "messages_robocup_ssl_detection.proto"; + +message Log_Frame +{ + required SSL_DetectionFrame frame = 1; + required string refbox_cmd = 2; +} + +message Refbox_Log +{ + repeated Log_Frame log = 1; +} diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto new file mode 100644 index 0000000..328a0d9 --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto @@ -0,0 +1,12 @@ +syntax = "proto2"; + +message Robots_Status{ + repeated Robot_Status robots_status = 1; +} + +message Robot_Status { + required int32 robot_id = 1; + required bool infrared = 2; + required bool flat_kick = 3; + required bool chip_kick = 4; +} \ No newline at end of file diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto new file mode 100644 index 0000000..4b4e3e2 --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto @@ -0,0 +1,8 @@ +syntax = "proto2"; +import "messages_robocup_ssl_detection.proto"; +import "messages_robocup_ssl_geometry.proto"; + +message SSL_WrapperPacket { + optional SSL_DetectionFrame detection = 1; + optional SSL_GeometryData geometry = 2; +} diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto new file mode 100755 index 0000000..0e62ad7 --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto @@ -0,0 +1,164 @@ +syntax = "proto2"; + +import "game_event.proto"; + +// Each UDP packet contains one of these messages. +message SSL_Referee { + // The UNIX timestamp when the packet was sent, in microseconds. + // Divide by 1,000,000 to get a time_t. + required uint64 packet_timestamp = 1; + + // These are the "coarse" stages of the game. + enum Stage { + // The first half is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + NORMAL_FIRST_HALF_PRE = 0; + // The first half of the normal game, before half time. + NORMAL_FIRST_HALF = 1; + // Half time between first and second halves. + NORMAL_HALF_TIME = 2; + // The second half is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + NORMAL_SECOND_HALF_PRE = 3; + // The second half of the normal game, after half time. + NORMAL_SECOND_HALF = 4; + // The break before extra time. + EXTRA_TIME_BREAK = 5; + // The first half of extra time is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + EXTRA_FIRST_HALF_PRE = 6; + // The first half of extra time. + EXTRA_FIRST_HALF = 7; + // Half time between first and second extra halves. + EXTRA_HALF_TIME = 8; + // The second half of extra time is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + EXTRA_SECOND_HALF_PRE = 9; + // The second half of extra time. + EXTRA_SECOND_HALF = 10; + // The break before penalty shootout. + PENALTY_SHOOTOUT_BREAK = 11; + // The penalty shootout. + PENALTY_SHOOTOUT = 12; + // The game is over. + POST_GAME = 13; + } + required Stage stage = 2; + + // The number of microseconds left in the stage. + // The following stages have this value; the rest do not: + // NORMAL_FIRST_HALF + // NORMAL_HALF_TIME + // NORMAL_SECOND_HALF + // EXTRA_TIME_BREAK + // EXTRA_FIRST_HALF + // EXTRA_HALF_TIME + // EXTRA_SECOND_HALF + // PENALTY_SHOOTOUT_BREAK + // + // If the stage runs over its specified time, this value + // becomes negative. + optional sint32 stage_time_left = 3; + + // These are the "fine" states of play on the field. + enum Command { + // All robots should completely stop moving. + HALT = 0; + // Robots must keep 50 cm from the ball. + STOP = 1; + // A prepared kickoff or penalty may now be taken. + NORMAL_START = 2; + // The ball is dropped and free for either team. + FORCE_START = 3; + // The yellow team may move into kickoff position. + PREPARE_KICKOFF_YELLOW = 4; + // The blue team may move into kickoff position. + PREPARE_KICKOFF_BLUE = 5; + // The yellow team may move into penalty position. + PREPARE_PENALTY_YELLOW = 6; + // The blue team may move into penalty position. + PREPARE_PENALTY_BLUE = 7; + // The yellow team may take a direct free kick. + DIRECT_FREE_YELLOW = 8; + // The blue team may take a direct free kick. + DIRECT_FREE_BLUE = 9; + // The yellow team may take an indirect free kick. + INDIRECT_FREE_YELLOW = 10; + // The blue team may take an indirect free kick. + INDIRECT_FREE_BLUE = 11; + // The yellow team is currently in a timeout. + TIMEOUT_YELLOW = 12; + // The blue team is currently in a timeout. + TIMEOUT_BLUE = 13; + // The yellow team just scored a goal. + // For information only. + // For rules compliance, teams must treat as STOP. + GOAL_YELLOW = 14; + // The blue team just scored a goal. + GOAL_BLUE = 15; + // Equivalent to STOP, but the yellow team must pick up the ball and + // drop it in the Designated Position. + BALL_PLACEMENT_YELLOW = 16; + // Equivalent to STOP, but the blue team must pick up the ball and drop + // it in the Designated Position. + BALL_PLACEMENT_BLUE = 17; + } + required Command command = 4; + + // The number of commands issued since startup (mod 2^32). + required uint32 command_counter = 5; + + // The UNIX timestamp when the command was issued, in microseconds. + // This value changes only when a new command is issued, not on each packet. + required uint64 command_timestamp = 6; + + // Information about a single team. + message TeamInfo { + // The team's name (empty string if operator has not typed anything). + required string name = 1; + // The number of goals scored by the team during normal play and overtime. + required uint32 score = 2; + // The number of red cards issued to the team since the beginning of the game. + required uint32 red_cards = 3; + // The amount of time (in microseconds) left on each yellow card issued to the team. + // If no yellow cards are issued, this array has no elements. + // Otherwise, times are ordered from smallest to largest. + repeated uint32 yellow_card_times = 4 [packed=true]; + // The total number of yellow cards ever issued to the team. + required uint32 yellow_cards = 5; + // The number of timeouts this team can still call. + // If in a timeout right now, that timeout is excluded. + required uint32 timeouts = 6; + // The number of microseconds of timeout this team can use. + required uint32 timeout_time = 7; + // The pattern number of this team's goalie. + required uint32 goalie = 8; + } + + // Information about the two teams. + required TeamInfo yellow = 7; + required TeamInfo blue = 8; + + // The coordinates of the Designated Position. These are measured in + // millimetres and correspond to SSL-Vision coordinates. These fields are + // always either both present (in the case of a ball placement command) or + // both absent (in the case of any other command). + message Point { + required float x = 1; + required float y = 2; + } + optional Point designated_position = 9; + + // Information about the direction of play. + // True, if the blue team will have it's goal on the positive x-axis of the ssl-vision coordinate system + // Obviously, the yellow team will play on the opposide half + // For compatibility, this field is optional + optional bool blueTeamOnPositiveHalf = 10; + + // The game event that caused the referee command + optional SSL_Referee_Game_Event gameEvent = 11; +} diff --git a/consai2r2_receiver/include/multicast.hpp b/consai2r2_receiver/include/multicast.hpp new file mode 100644 index 0000000..7889e10 --- /dev/null +++ b/consai2r2_receiver/include/multicast.hpp @@ -0,0 +1,44 @@ +#ifndef CONSAI2R2_MULTICAST_HPP_ +#define CONSAI2R2_MULTICAST_HPP_ + +#include +#include +#include +#include + +namespace asio = boost::asio; + +class MulticastReceiver { + public: + MulticastReceiver(const std::string& ip, const int port) + : endpoint(asio::ip::udp::v4(), port), socket(io_service, endpoint) { + asio::ip::address addr = asio::ip::address::from_string(ip); + if (!addr.is_multicast()) { + throw std::runtime_error("excpeted multicast address"); + } + + socket.set_option(asio::ip::multicast::join_group(addr.to_v4())); + socket.set_option(asio::socket_base::reuse_address(true)); + socket.non_blocking(true); + } + + size_t receive(std::vector& msg) { + boost::system::error_code error; + const size_t received = + socket.receive_from(asio::buffer(msg), endpoint, 0, error); + if (error && error != asio::error::message_size) { + throw boost::system::system_error(error); + return 0; + } + return received; + } + + size_t available() { return socket.available(); } + + private: + asio::io_service io_service; + asio::ip::udp::endpoint endpoint; + asio::ip::udp::socket socket; +}; + +#endif diff --git a/consai2r2_receiver/package.xml b/consai2r2_receiver/package.xml new file mode 100644 index 0000000..b86c97c --- /dev/null +++ b/consai2r2_receiver/package.xml @@ -0,0 +1,24 @@ + + + + consai2r2_receiver + 0.0.0 + Receive messages from SSL-Vision, SSL-Referee and grSim + akshota + MIT + + ament_cmake + + rclcpp + std_msgs + consai2r2_msgs + protobuf-dev + boost + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/consai2r2_receiver/src/referee_receiver.cpp b/consai2r2_receiver/src/referee_receiver.cpp new file mode 100644 index 0000000..2d7582c --- /dev/null +++ b/consai2r2_receiver/src/referee_receiver.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include + +#include "consai2r2_msgs/msg/referee.hpp" +#include "rclcpp/rclcpp.hpp" +#include "referee.pb.h" + +#include "multicast.hpp" + +using std::placeholders::_1; +using namespace std::chrono_literals; + +class RefereeReceiver : public rclcpp::Node +{ +public: + RefereeReceiver() : Node("consai2r2_referee_receiver") + { + pub = this->create_publisher("~/raw_referee", 1); + + this->declare_parameter("referee_addr", "224.5.23.1"); + this->declare_parameter("referee_port", 10003); + + this->get_parameter("referee_addr", this->referee_addr); + this->get_parameter("referee_port", this->referee_port); + + timer_ = create_wall_timer(16ms, std::bind(&RefereeReceiver::timer_callback, this)); + + receiver = std::unique_ptr( + new MulticastReceiver(this->referee_addr, this->referee_port)); + } + +private: + void timer_callback() + { + if (receiver->available()) { + std::vector buf(2048); + const size_t size = receiver->receive(buf); + + if (size > 0) { + SSL_Referee packet; + packet.ParseFromString(std::string(buf.begin(), buf.end())); + this->publish(packet); + } + } + } + + void convert_team_info( + const SSL_Referee_TeamInfo & proto_team, consai2r2_msgs::msg::RefereeTeamInfo & team_info) + { + team_info.name = proto_team.name(); + + team_info.score = proto_team.score(); + team_info.red_cards = proto_team.red_cards(); + + auto data = proto_team.yellow_card_times().data(); + std::vector v(data, data + proto_team.yellow_card_times().size()); + + team_info.yellow_card_times = v; + team_info.yellow_cards = proto_team.yellow_cards(); + team_info.timeouts = proto_team.timeouts(); + team_info.timeout_time = proto_team.timeout_time(); + team_info.goalie = proto_team.goalie(); + } + + void publish(const SSL_Referee & packet) + { + auto msg = consai2r2_msgs::msg::Referee(); + + // Convert Protobuf msg to ROS2 msg + msg.stage = packet.stage(); + msg.command = packet.command(); + msg.command_counter = packet.command_counter(); + + this->convert_team_info(packet.blue(), msg.blue); + this->convert_team_info(packet.yellow(), msg.yellow); + + if (packet.has_designated_position()) { + auto position = packet.designated_position(); + msg.designated_position.x = position.x() * 0.001; // millimeter to meter + msg.designated_position.y = position.y() * 0.001; // millimeter to meter + msg.designated_position.z = 0.0; + } + + if (packet.has_gameevent()) { + auto gameEvent = packet.gameevent(); + msg.game_event.game_event_type = gameEvent.gameeventtype(); + msg.game_event.originator_team = gameEvent.originator().team(); + msg.game_event.originator_bot_id = gameEvent.originator().botid(); + msg.game_event.message = gameEvent.message(); + } + + this->pub->publish(msg); + } + + rclcpp::Publisher::SharedPtr pub; + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr receiver; + std::string referee_addr; + int referee_port; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From d031d58e7fbbf7439e7a6183c642d8193c461563 Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Tue, 10 Dec 2019 22:37:21 +0900 Subject: [PATCH 03/13] change path of multicast.hpp --- .../include/consai2r2_receiver/multicast.hpp | 68 +++++++++++++++++++ consai2r2_receiver/include/multicast.hpp | 44 ------------ consai2r2_receiver/src/referee_receiver.cpp | 29 +++++++- 3 files changed, 94 insertions(+), 47 deletions(-) create mode 100644 consai2r2_receiver/include/consai2r2_receiver/multicast.hpp delete mode 100644 consai2r2_receiver/include/multicast.hpp diff --git a/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp b/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp new file mode 100644 index 0000000..475b182 --- /dev/null +++ b/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2019 SSL-Roots +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef CONSAI2R2_RECEIVER__MULTICAST_HPP_ +#define CONSAI2R2_RECEIVER__MULTICAST_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace asio = boost::asio; + +class MulticastReceiver +{ +public: + MulticastReceiver(const std::string & ip, const int port) + : endpoint(asio::ip::udp::v4(), port), socket(io_service, endpoint) + { + asio::ip::address addr = asio::ip::address::from_string(ip); + if (!addr.is_multicast()) { + throw std::runtime_error("excpeted multicast address"); + } + + socket.set_option(asio::ip::multicast::join_group(addr.to_v4())); + socket.set_option(asio::socket_base::reuse_address(true)); + socket.non_blocking(true); + } + + size_t receive(std::vector & msg) + { + boost::system::error_code error; + const size_t received = socket.receive_from(asio::buffer(msg), endpoint, 0, error); + if (error && error != asio::error::message_size) { + throw boost::system::system_error(error); + return 0; + } + return received; + } + + size_t available() {return socket.available();} + +private: + asio::io_service io_service; + asio::ip::udp::endpoint endpoint; + asio::ip::udp::socket socket; +}; + +#endif // CONSAI2R2_RECEIVER__MULTICAST_HPP_ diff --git a/consai2r2_receiver/include/multicast.hpp b/consai2r2_receiver/include/multicast.hpp deleted file mode 100644 index 7889e10..0000000 --- a/consai2r2_receiver/include/multicast.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef CONSAI2R2_MULTICAST_HPP_ -#define CONSAI2R2_MULTICAST_HPP_ - -#include -#include -#include -#include - -namespace asio = boost::asio; - -class MulticastReceiver { - public: - MulticastReceiver(const std::string& ip, const int port) - : endpoint(asio::ip::udp::v4(), port), socket(io_service, endpoint) { - asio::ip::address addr = asio::ip::address::from_string(ip); - if (!addr.is_multicast()) { - throw std::runtime_error("excpeted multicast address"); - } - - socket.set_option(asio::ip::multicast::join_group(addr.to_v4())); - socket.set_option(asio::socket_base::reuse_address(true)); - socket.non_blocking(true); - } - - size_t receive(std::vector& msg) { - boost::system::error_code error; - const size_t received = - socket.receive_from(asio::buffer(msg), endpoint, 0, error); - if (error && error != asio::error::message_size) { - throw boost::system::system_error(error); - return 0; - } - return received; - } - - size_t available() { return socket.available(); } - - private: - asio::io_service io_service; - asio::ip::udp::endpoint endpoint; - asio::ip::udp::socket socket; -}; - -#endif diff --git a/consai2r2_receiver/src/referee_receiver.cpp b/consai2r2_receiver/src/referee_receiver.cpp index 2d7582c..205f256 100644 --- a/consai2r2_receiver/src/referee_receiver.cpp +++ b/consai2r2_receiver/src/referee_receiver.cpp @@ -1,14 +1,36 @@ +// Copyright (c) 2019 SSL-Roots +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + #include #include #include #include #include +#include +#include +#include "./referee.pb.h" #include "consai2r2_msgs/msg/referee.hpp" #include "rclcpp/rclcpp.hpp" -#include "referee.pb.h" -#include "multicast.hpp" +#include "consai2r2_receiver/multicast.hpp" using std::placeholders::_1; using namespace std::chrono_literals; @@ -16,7 +38,8 @@ using namespace std::chrono_literals; class RefereeReceiver : public rclcpp::Node { public: - RefereeReceiver() : Node("consai2r2_referee_receiver") + RefereeReceiver() + : Node("consai2r2_referee_receiver") { pub = this->create_publisher("~/raw_referee", 1); From 8b78e5c28a263bcc24bbe3a7d67ec4c43ef315a5 Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Tue, 10 Dec 2019 22:37:31 +0900 Subject: [PATCH 04/13] fix CMakeLists.txt --- consai2r2_receiver/CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index d12f551..a20b7d3 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(consai2r2_msgs REQUIRED) find_package(Protobuf REQUIRED) -find_package(Boost REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) # Protobuf include_directories( @@ -36,7 +36,7 @@ protobuf_generate_cpp(PROTO_CPP PROTO_H ) include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) -add_executable(referee_receiver +add_executable(referee_receiver src/referee_receiver.cpp ${PROTO_CPP} ${PROTO_H} @@ -50,6 +50,10 @@ ament_target_dependencies( target_link_libraries(referee_receiver ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES}) +install( + DIRECTORY include/ + DESTINATION include) + install(TARGETS referee_receiver EXPORT export_${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) From db02ad5b5d8f221ae69d4c5a048afd2b362244fa Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Tue, 10 Dec 2019 22:44:30 +0900 Subject: [PATCH 05/13] delete duplicated include path --- consai2r2_receiver/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index a20b7d3..6898444 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(Boost REQUIRED COMPONENTS system) # Protobuf include_directories( - "include" ${PROTOBUF_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ) From b9564a8406cf530d62cf69ad5f0ab2de88b397ab Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Sat, 7 Dec 2019 22:38:27 +0900 Subject: [PATCH 06/13] implement VisionReceiver --- consai2r2_msgs/CMakeLists.txt | 11 ++ consai2r2_msgs/msg/BallInfo.msg | 10 + consai2r2_msgs/msg/ControlTarget.msg | 21 ++ consai2r2_msgs/msg/DecodedReferee.msg | 40 ++++ consai2r2_msgs/msg/DetectionBall.msg | 3 + consai2r2_msgs/msg/DetectionFrame.msg | 8 + consai2r2_msgs/msg/DetectionRobot.msg | 3 + consai2r2_msgs/msg/FieldCircularArc.msg | 20 ++ consai2r2_msgs/msg/FieldLineSegment.msg | 15 ++ consai2r2_msgs/msg/ReplaceBall.msg | 8 + consai2r2_msgs/msg/RobotInfo.msg | 29 +++ consai2r2_msgs/msg/VisionDetections.msg | 2 + consai2r2_msgs/msg/VisionGeometry.msg | 9 + consai2r2_receiver/CMakeLists.txt | 16 +- consai2r2_receiver/src/vision_receiver.cpp | 218 +++++++++++++++++++++ 15 files changed, 412 insertions(+), 1 deletion(-) create mode 100755 consai2r2_msgs/msg/BallInfo.msg create mode 100755 consai2r2_msgs/msg/ControlTarget.msg create mode 100755 consai2r2_msgs/msg/DecodedReferee.msg create mode 100755 consai2r2_msgs/msg/DetectionBall.msg create mode 100755 consai2r2_msgs/msg/DetectionFrame.msg create mode 100755 consai2r2_msgs/msg/DetectionRobot.msg create mode 100755 consai2r2_msgs/msg/FieldCircularArc.msg create mode 100755 consai2r2_msgs/msg/FieldLineSegment.msg create mode 100755 consai2r2_msgs/msg/ReplaceBall.msg create mode 100755 consai2r2_msgs/msg/RobotInfo.msg create mode 100755 consai2r2_msgs/msg/VisionDetections.msg create mode 100755 consai2r2_msgs/msg/VisionGeometry.msg create mode 100644 consai2r2_receiver/src/vision_receiver.cpp diff --git a/consai2r2_msgs/CMakeLists.txt b/consai2r2_msgs/CMakeLists.txt index 817c082..330d72f 100644 --- a/consai2r2_msgs/CMakeLists.txt +++ b/consai2r2_msgs/CMakeLists.txt @@ -22,6 +22,17 @@ set(msg_files "msg/RefereeGameEvent.msg" "msg/RefereeTeamInfo.msg" "msg/Referee.msg" + "msg/FieldCircularArc.msg" + "msg/FieldLineSegment.msg" + "msg/VisionGeometry.msg" + "msg/DetectionBall.msg" + "msg/DetectionRobot.msg" + "msg/DetectionFrame.msg" + "msg/VisionDetections.msg" + "msg/RobotInfo.msg" + "msg/BallInfo.msg" + "msg/DecodedReferee.msg" + "msg/ReplaceBall.msg" "msg/RobotCommand.msg" "msg/RobotCommands.msg" ) diff --git a/consai2r2_msgs/msg/BallInfo.msg b/consai2r2_msgs/msg/BallInfo.msg new file mode 100755 index 0000000..6a268cd --- /dev/null +++ b/consai2r2_msgs/msg/BallInfo.msg @@ -0,0 +1,10 @@ +# ball information for consai2_game + +geometry_msgs/Pose2D pose +geometry_msgs/Pose2D velocity +geometry_msgs/Twist velocity_twist +bool detected +builtin_interfaces/Time detection_stamp + +geometry_msgs/Pose2D last_detection_pose +bool disappeared diff --git a/consai2r2_msgs/msg/ControlTarget.msg b/consai2r2_msgs/msg/ControlTarget.msg new file mode 100755 index 0000000..d1a0ff7 --- /dev/null +++ b/consai2r2_msgs/msg/ControlTarget.msg @@ -0,0 +1,21 @@ +# control target for consai2_game and consai2_control + +uint8 robot_id + +bool control_enable +# path[last] == goal_pose +geometry_msgs/Pose2D[] path +geometry_msgs/Pose2D goal_velocity + +# 0.0 ~ 1.0 +float64 kick_power +# True/False +bool chip_enable +# 0.0 ~ 1.0 +float64 dribble_power + +# default:unused +bool test_flag +# default:unused +float64 test_param + diff --git a/consai2r2_msgs/msg/DecodedReferee.msg b/consai2r2_msgs/msg/DecodedReferee.msg new file mode 100755 index 0000000..57a6d82 --- /dev/null +++ b/consai2r2_msgs/msg/DecodedReferee.msg @@ -0,0 +1,40 @@ +# game status for consai2_game + +# ID : text +# 0 : HALT +# 1 : STOP +# 2 : --- +# 3 : FORCE_START +# 11 : OUR_KICKOFF_PREPARATION +# 12 : OUR_KICKOFF_START +# 13 : OUR_PENALTY_PREPARATION +# 14 : OUR_PENALTY_START +# 15 : OUR_DIRECT_FREE +# 16 : OUR_INDIRECT_FREE +# 17 : OUR_TIMEOUT +# 18 : OUR_GOAL +# 19 : OUR_BALL_PLACEMENT +# 20 : --- +# 21 : THEIR_KICKOFF_PREPARATION +# 22 : THEIR_KICKOFF_START +# 23 : THEIR_PENALTY_PREPARATION +# 24 : THEIR_PENALTY_START +# 25 : THEIR_DIRECT_FREE +# 26 : THEIR_INDIRECT_FREE +# 27 : THEIR_TIMEOUT +# 28 : THEIR_GOAL +# 29 : THEIR_BALL_PLACEMENT + +uint8 referee_id +string referee_text + +bool can_move_robot +float64 speed_limit_of_robot # meter/sec +bool can_kick_ball +bool can_enter_their_side +bool can_enter_center_circle +bool is_inplay +float64 keep_out_radius_from_ball # meter +float64 keep_out_distance_from_their_defense_area # meter + +geometry_msgs/Point placement_position diff --git a/consai2r2_msgs/msg/DetectionBall.msg b/consai2r2_msgs/msg/DetectionBall.msg new file mode 100755 index 0000000..0d9d60e --- /dev/null +++ b/consai2r2_msgs/msg/DetectionBall.msg @@ -0,0 +1,3 @@ +# This msg file is copied from 'messages_robocup_ssl_detection.proto' + +geometry_msgs/Pose2D pose diff --git a/consai2r2_msgs/msg/DetectionFrame.msg b/consai2r2_msgs/msg/DetectionFrame.msg new file mode 100755 index 0000000..d6cd5c3 --- /dev/null +++ b/consai2r2_msgs/msg/DetectionFrame.msg @@ -0,0 +1,8 @@ +# This msg file is copied from 'messages_robocup_ssl_detection.proto' + +float64 t_capture +float64 t_sent +uint32 camera_id +DetectionBall[] balls +DetectionRobot[] robots_yellow +DetectionRobot[] robots_blue diff --git a/consai2r2_msgs/msg/DetectionRobot.msg b/consai2r2_msgs/msg/DetectionRobot.msg new file mode 100755 index 0000000..d7591a2 --- /dev/null +++ b/consai2r2_msgs/msg/DetectionRobot.msg @@ -0,0 +1,3 @@ +# This msg file is copied from 'messages_robocup_ssl_detection.proto' +geometry_msgs/Pose2D pose +uint8 robot_id diff --git a/consai2r2_msgs/msg/FieldCircularArc.msg b/consai2r2_msgs/msg/FieldCircularArc.msg new file mode 100755 index 0000000..29f109f --- /dev/null +++ b/consai2r2_msgs/msg/FieldCircularArc.msg @@ -0,0 +1,20 @@ +# This msg file is copied from 'messages_robocup_ssl_geometry.proto' + +# Name of this field marking. +string name + +# Center point of the circular arc. +float64 center_x +float64 center_y + +# Radius of the arc. +float64 radius + +# Start angle in counter-clockwise order. +float64 a1 + +# End angle in counter-clockwise order. +float64 a2 + +# Thickness of the arc. +float64 thickness diff --git a/consai2r2_msgs/msg/FieldLineSegment.msg b/consai2r2_msgs/msg/FieldLineSegment.msg new file mode 100755 index 0000000..8885e92 --- /dev/null +++ b/consai2r2_msgs/msg/FieldLineSegment.msg @@ -0,0 +1,15 @@ +# This msg file is copied from 'messages_robocup_ssl_geometry.proto' + +# Name of this field marking. +string name + +# Start point of the line segment. +float64 p1_x +float64 p1_y + +# End point of the line segment. +float64 p2_x +float64 p2_y + +# Thickness of the line segment. +float64 thickness diff --git a/consai2r2_msgs/msg/ReplaceBall.msg b/consai2r2_msgs/msg/ReplaceBall.msg new file mode 100755 index 0000000..546df12 --- /dev/null +++ b/consai2r2_msgs/msg/ReplaceBall.msg @@ -0,0 +1,8 @@ +# This msg file is copied from 'grSim_Replacement.proto' +# +float64 x +float64 y +float64 vx +float64 vy + +bool is_enabled diff --git a/consai2r2_msgs/msg/RobotInfo.msg b/consai2r2_msgs/msg/RobotInfo.msg new file mode 100755 index 0000000..fa1b49f --- /dev/null +++ b/consai2r2_msgs/msg/RobotInfo.msg @@ -0,0 +1,29 @@ +# robot information for consai2_game + +# robot id +uint8 robot_id + +# Robot 2D pose +# This field will be replaced to geometry_msgs/Pose +geometry_msgs/Pose2D pose + +# Robot Velocity +# [Deprecated] This field will be removed. +geometry_msgs/Pose2D velocity + +# Robot Velocity +geometry_msgs/Twist velocity_twist + +# If robot is detected by vision in this frame, this field is set to true +bool detected + +# Latest vision detection time +# [Deprecated] This field will be removed. +builtin_interfaces/Time detection_stamp + +# Latest detected pose. +# This field is NOT filtered. This is a raw vision value. +geometry_msgs/Pose2D last_detection_pose + +# If a robot disappear from field, this field is set to true +bool disappeared diff --git a/consai2r2_msgs/msg/VisionDetections.msg b/consai2r2_msgs/msg/VisionDetections.msg new file mode 100755 index 0000000..90c1086 --- /dev/null +++ b/consai2r2_msgs/msg/VisionDetections.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +DetectionFrame[] frames diff --git a/consai2r2_msgs/msg/VisionGeometry.msg b/consai2r2_msgs/msg/VisionGeometry.msg new file mode 100755 index 0000000..860a023 --- /dev/null +++ b/consai2r2_msgs/msg/VisionGeometry.msg @@ -0,0 +1,9 @@ +# This msg file is copied from 'messages_robocup_ssl_geometry.proto' + +float64 field_length +float64 field_width +float64 goal_width +float64 goal_depth +int32 boundary_width +FieldLineSegment[] field_lines +FieldCircularArc[] field_arcs diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index 6898444..9bc0037 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -40,14 +40,25 @@ add_executable(referee_receiver ${PROTO_CPP} ${PROTO_H} ) +add_executable(vision_receiver + src/vision_receiver.cpp + ${PROTO_CPP} + ${PROTO_H} +) ament_target_dependencies( referee_receiver "rclcpp" "std_msgs" "consai2r2_msgs" ) +ament_target_dependencies( + vision_receiver + "rclcpp" + "std_msgs" + "consai2r2_msgs" +) target_link_libraries(referee_receiver ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES}) - +target_link_libraries(vision_receiver ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES}) install( DIRECTORY include/ @@ -56,6 +67,9 @@ install( install(TARGETS referee_receiver EXPORT export_${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) +install(TARGETS vision_receiver + EXPORT export_${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/consai2r2_receiver/src/vision_receiver.cpp b/consai2r2_receiver/src/vision_receiver.cpp new file mode 100644 index 0000000..94f92b6 --- /dev/null +++ b/consai2r2_receiver/src/vision_receiver.cpp @@ -0,0 +1,218 @@ +// Copyright (c) 2019 SSL-Roots +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include +#include +#include +#include +#include +#include + +#include "./messages_robocup_ssl_wrapper.pb.h" +#include "consai2r2_msgs/msg/vision_detections.hpp" +#include "consai2r2_msgs/msg/vision_geometry.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "consai2r2_receiver/multicast.hpp" + +using std::placeholders::_1; +using namespace std::chrono_literals; + +static const double TO_METER = 0.001; + +class FormatConverter +{ +public: + FormatConverter() {vision_detections = consai2r2_msgs::msg::VisionDetections();} + + void append_frame(const SSL_DetectionFrame & packet_detection) + { + auto detection_frame = consai2r2_msgs::msg::DetectionFrame(); + + detection_frame.t_capture = packet_detection.t_capture(); + detection_frame.t_sent = packet_detection.t_sent(); + detection_frame.camera_id = packet_detection.camera_id(); + + // ball + auto balls = packet_detection.balls(); + for (auto ball = balls.begin(); ball != balls.end(); ball++) { + detection_frame.balls.push_back(this->convert_to_ball_topic(*ball)); + } + + // blue robot + auto robots_blue = packet_detection.robots_blue(); + for (auto robot = robots_blue.begin(); robot != robots_blue.end(); robot++) { + detection_frame.robots_blue.push_back(this->convert_to_robot_topic(*robot)); + } + + // yellow robot + auto robots_yellow = packet_detection.robots_yellow(); + for (auto robot = robots_yellow.begin(); robot != robots_yellow.end(); robot++) { + detection_frame.robots_yellow.push_back(this->convert_to_robot_topic(*robot)); + } + + this->vision_detections.frames.push_back(detection_frame); + } + + void frame_clear() {this->vision_detections.frames.clear();} + + consai2r2_msgs::msg::VisionDetections get_frame() {return this->vision_detections;} + +private: + consai2r2_msgs::msg::DetectionBall convert_to_ball_topic(const SSL_DetectionBall & raw_ball) + { + auto detection_ball = consai2r2_msgs::msg::DetectionBall(); + + detection_ball.pose.x = raw_ball.x() * TO_METER; + detection_ball.pose.y = raw_ball.y() * TO_METER; + + return detection_ball; + } + + consai2r2_msgs::msg::DetectionRobot convert_to_robot_topic(const SSL_DetectionRobot & raw_robot) + { + auto detection_robot = consai2r2_msgs::msg::DetectionRobot(); + + detection_robot.robot_id = raw_robot.robot_id(); + detection_robot.pose.x = raw_robot.x() * TO_METER; + detection_robot.pose.y = raw_robot.y() * TO_METER; + + return detection_robot; + } + + consai2r2_msgs::msg::VisionDetections vision_detections; +}; + +class VisionReceiver : public rclcpp::Node +{ +public: + VisionReceiver() + : Node("consai2r2_vision_receiver"), converter() + { + std::string host; + int port; + + pub_geometry = + this->create_publisher("~/raw_vision_geometry", 1); + pub_detection = + this->create_publisher("~/raw_vision_detections", 1); + + this->declare_parameter("vision_addr", "224.5.23.2"); + this->declare_parameter("vision_port", 10006); + + this->get_parameter("vision_addr", host); + this->get_parameter("vision_port", port); + + timer_ = create_wall_timer(16ms, std::bind(&VisionReceiver::timer_callback, this)); + + receiver = std::unique_ptr(new MulticastReceiver(host, port)); + } + +private: + void timer_callback() + { + this->converter.frame_clear(); + + while (receiver->available()) { + std::vector buf(2048); + const size_t size = receiver->receive(buf); + + if (size > 0) { + SSL_WrapperPacket packet; + packet.ParseFromString(std::string(buf.begin(), buf.end())); + + if (packet.has_detection()) { + this->converter.append_frame(packet.detection()); + } + if (packet.has_geometry()) { + this->publish_geometry(packet.geometry().field()); + } + } + } + this->publish_detection(); + } + + void publish_detection() + { + auto vision_detection = this->converter.get_frame(); + + if (vision_detection.frames.size() > 0) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + vision_detection.header.stamp = ros_clock.now(); + this->pub_detection->publish(vision_detection); + } + } + + void publish_geometry(const SSL_GeometryFieldSize & packet_field) + { + auto geometry = consai2r2_msgs::msg::VisionGeometry(); + + geometry.field_length = packet_field.field_length() * TO_METER; + geometry.field_width = packet_field.field_width() * TO_METER; + geometry.goal_width = packet_field.goal_width() * TO_METER; + geometry.goal_depth = packet_field.goal_depth() * TO_METER; + geometry.boundary_width = static_cast(packet_field.boundary_width() * TO_METER); + + auto lines = packet_field.field_lines(); + for (auto line = lines.begin(); line != lines.end(); line++) { + auto line_segment = consai2r2_msgs::msg::FieldLineSegment(); + + line_segment.name = line->name(); + line_segment.p1_x = line->p1().x() * TO_METER; + line_segment.p1_y = line->p1().y() * TO_METER; + line_segment.p2_x = line->p2().x() * TO_METER; + line_segment.p2_y = line->p2().y() * TO_METER; + line_segment.thickness = line->thickness() * TO_METER; + geometry.field_lines.push_back(line_segment); + } + + auto arcs = packet_field.field_arcs(); + for (auto arc = arcs.begin(); arc != arcs.end(); arc++) { + auto circular_arc = consai2r2_msgs::msg::FieldCircularArc(); + + circular_arc.name = arc->name(); + circular_arc.center_x = arc->center().x() * TO_METER; + circular_arc.center_y = arc->center().y() * TO_METER; + circular_arc.radius = arc->radius() * TO_METER; + circular_arc.a1 = arc->a1(); + circular_arc.a2 = arc->a2(); + circular_arc.thickness = arc->thickness() * TO_METER; + geometry.field_arcs.push_back(circular_arc); + } + + this->pub_geometry->publish(geometry); + } + + rclcpp::Publisher::SharedPtr pub_geometry; + rclcpp::Publisher::SharedPtr pub_detection; + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr receiver; + FormatConverter converter; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 8ad1115b17dadde87054d7c188d797b38e17b4c4 Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Thu, 12 Dec 2019 21:49:30 +0900 Subject: [PATCH 07/13] move .proto files to src/ --- consai2r2_receiver/CMakeLists.txt | 14 +++++++------- .../proto/game_event.proto | 0 .../proto/messages_robocup_ssl_detection.proto | 0 .../proto/messages_robocup_ssl_geometry.proto | 0 .../proto/messages_robocup_ssl_refbox_log.proto | 0 .../proto/messages_robocup_ssl_robot_status.proto | 0 .../proto/messages_robocup_ssl_wrapper.proto | 0 .../consai2r2_receiver => src}/proto/referee.proto | 0 8 files changed, 7 insertions(+), 7 deletions(-) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/game_event.proto (100%) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/messages_robocup_ssl_detection.proto (100%) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/messages_robocup_ssl_geometry.proto (100%) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/messages_robocup_ssl_refbox_log.proto (100%) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/messages_robocup_ssl_robot_status.proto (100%) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/messages_robocup_ssl_wrapper.proto (100%) rename consai2r2_receiver/{include/consai2r2_receiver => src}/proto/referee.proto (100%) diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index 6898444..0dc9f5a 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -25,13 +25,13 @@ include_directories( ) protobuf_generate_cpp(PROTO_CPP PROTO_H - include/${PROJECT_NAME}/proto/messages_robocup_ssl_wrapper.proto - include/${PROJECT_NAME}/proto/messages_robocup_ssl_geometry.proto - include/${PROJECT_NAME}/proto/messages_robocup_ssl_detection.proto - include/${PROJECT_NAME}/proto/messages_robocup_ssl_robot_status.proto - include/${PROJECT_NAME}/proto/messages_robocup_ssl_refbox_log.proto - include/${PROJECT_NAME}/proto/referee.proto - include/${PROJECT_NAME}/proto/game_event.proto + src/proto/messages_robocup_ssl_wrapper.proto + src/proto/messages_robocup_ssl_geometry.proto + src/proto/messages_robocup_ssl_detection.proto + src/proto/messages_robocup_ssl_robot_status.proto + src/proto/messages_robocup_ssl_refbox_log.proto + src/proto/referee.proto + src/proto/game_event.proto ) include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto b/consai2r2_receiver/src/proto/game_event.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto rename to consai2r2_receiver/src/proto/game_event.proto diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto diff --git a/consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto b/consai2r2_receiver/src/proto/referee.proto similarity index 100% rename from consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto rename to consai2r2_receiver/src/proto/referee.proto From 7429b8df92c5c3ac94607d2ee40ef7822de005ca Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Thu, 12 Dec 2019 22:18:12 +0900 Subject: [PATCH 08/13] move .proto files to proto/ --- consai2r2_receiver/CMakeLists.txt | 14 +++++++------- .../{src => }/proto/game_event.proto | 0 .../proto/messages_robocup_ssl_detection.proto | 0 .../proto/messages_robocup_ssl_geometry.proto | 0 .../proto/messages_robocup_ssl_refbox_log.proto | 0 .../proto/messages_robocup_ssl_robot_status.proto | 0 .../proto/messages_robocup_ssl_wrapper.proto | 0 consai2r2_receiver/{src => }/proto/referee.proto | 0 8 files changed, 7 insertions(+), 7 deletions(-) rename consai2r2_receiver/{src => }/proto/game_event.proto (100%) rename consai2r2_receiver/{src => }/proto/messages_robocup_ssl_detection.proto (100%) rename consai2r2_receiver/{src => }/proto/messages_robocup_ssl_geometry.proto (100%) rename consai2r2_receiver/{src => }/proto/messages_robocup_ssl_refbox_log.proto (100%) rename consai2r2_receiver/{src => }/proto/messages_robocup_ssl_robot_status.proto (100%) rename consai2r2_receiver/{src => }/proto/messages_robocup_ssl_wrapper.proto (100%) rename consai2r2_receiver/{src => }/proto/referee.proto (100%) diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index 0dc9f5a..6732311 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -25,13 +25,13 @@ include_directories( ) protobuf_generate_cpp(PROTO_CPP PROTO_H - src/proto/messages_robocup_ssl_wrapper.proto - src/proto/messages_robocup_ssl_geometry.proto - src/proto/messages_robocup_ssl_detection.proto - src/proto/messages_robocup_ssl_robot_status.proto - src/proto/messages_robocup_ssl_refbox_log.proto - src/proto/referee.proto - src/proto/game_event.proto + proto/messages_robocup_ssl_wrapper.proto + proto/messages_robocup_ssl_geometry.proto + proto/messages_robocup_ssl_detection.proto + proto/messages_robocup_ssl_robot_status.proto + proto/messages_robocup_ssl_refbox_log.proto + proto/referee.proto + proto/game_event.proto ) include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/consai2r2_receiver/src/proto/game_event.proto b/consai2r2_receiver/proto/game_event.proto similarity index 100% rename from consai2r2_receiver/src/proto/game_event.proto rename to consai2r2_receiver/proto/game_event.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto b/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto rename to consai2r2_receiver/proto/messages_robocup_ssl_detection.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto b/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto rename to consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto b/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto rename to consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto b/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto rename to consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto b/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto rename to consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto diff --git a/consai2r2_receiver/src/proto/referee.proto b/consai2r2_receiver/proto/referee.proto similarity index 100% rename from consai2r2_receiver/src/proto/referee.proto rename to consai2r2_receiver/proto/referee.proto From df32daf554e15c550a6c95544d4778c66dec0ade Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Thu, 12 Dec 2019 23:16:52 +0900 Subject: [PATCH 09/13] Revert "move .proto files to proto/" This reverts commit 7429b8df92c5c3ac94607d2ee40ef7822de005ca. --- consai2r2_receiver/CMakeLists.txt | 14 +++++++------- .../{ => src}/proto/game_event.proto | 0 .../proto/messages_robocup_ssl_detection.proto | 0 .../proto/messages_robocup_ssl_geometry.proto | 0 .../proto/messages_robocup_ssl_refbox_log.proto | 0 .../proto/messages_robocup_ssl_robot_status.proto | 0 .../proto/messages_robocup_ssl_wrapper.proto | 0 consai2r2_receiver/{ => src}/proto/referee.proto | 0 8 files changed, 7 insertions(+), 7 deletions(-) rename consai2r2_receiver/{ => src}/proto/game_event.proto (100%) rename consai2r2_receiver/{ => src}/proto/messages_robocup_ssl_detection.proto (100%) rename consai2r2_receiver/{ => src}/proto/messages_robocup_ssl_geometry.proto (100%) rename consai2r2_receiver/{ => src}/proto/messages_robocup_ssl_refbox_log.proto (100%) rename consai2r2_receiver/{ => src}/proto/messages_robocup_ssl_robot_status.proto (100%) rename consai2r2_receiver/{ => src}/proto/messages_robocup_ssl_wrapper.proto (100%) rename consai2r2_receiver/{ => src}/proto/referee.proto (100%) diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index 6732311..0dc9f5a 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -25,13 +25,13 @@ include_directories( ) protobuf_generate_cpp(PROTO_CPP PROTO_H - proto/messages_robocup_ssl_wrapper.proto - proto/messages_robocup_ssl_geometry.proto - proto/messages_robocup_ssl_detection.proto - proto/messages_robocup_ssl_robot_status.proto - proto/messages_robocup_ssl_refbox_log.proto - proto/referee.proto - proto/game_event.proto + src/proto/messages_robocup_ssl_wrapper.proto + src/proto/messages_robocup_ssl_geometry.proto + src/proto/messages_robocup_ssl_detection.proto + src/proto/messages_robocup_ssl_robot_status.proto + src/proto/messages_robocup_ssl_refbox_log.proto + src/proto/referee.proto + src/proto/game_event.proto ) include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/consai2r2_receiver/proto/game_event.proto b/consai2r2_receiver/src/proto/game_event.proto similarity index 100% rename from consai2r2_receiver/proto/game_event.proto rename to consai2r2_receiver/src/proto/game_event.proto diff --git a/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto similarity index 100% rename from consai2r2_receiver/proto/messages_robocup_ssl_detection.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto diff --git a/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto similarity index 100% rename from consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto diff --git a/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto similarity index 100% rename from consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto diff --git a/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto similarity index 100% rename from consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto diff --git a/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto b/consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto similarity index 100% rename from consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto rename to consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto diff --git a/consai2r2_receiver/proto/referee.proto b/consai2r2_receiver/src/proto/referee.proto similarity index 100% rename from consai2r2_receiver/proto/referee.proto rename to consai2r2_receiver/src/proto/referee.proto From 8bcfcb77c336552300055506f792b67c7de00c80 Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Thu, 12 Dec 2019 23:20:24 +0900 Subject: [PATCH 10/13] Revert "move .proto files to src/" This reverts commit 8ad1115b17dadde87054d7c188d797b38e17b4c4. --- consai2r2_receiver/CMakeLists.txt | 14 +++++++------- .../consai2r2_receiver}/proto/game_event.proto | 0 .../proto/messages_robocup_ssl_detection.proto | 0 .../proto/messages_robocup_ssl_geometry.proto | 0 .../proto/messages_robocup_ssl_refbox_log.proto | 0 .../proto/messages_robocup_ssl_robot_status.proto | 0 .../proto/messages_robocup_ssl_wrapper.proto | 0 .../consai2r2_receiver}/proto/referee.proto | 0 8 files changed, 7 insertions(+), 7 deletions(-) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/game_event.proto (100%) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/messages_robocup_ssl_detection.proto (100%) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/messages_robocup_ssl_geometry.proto (100%) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/messages_robocup_ssl_refbox_log.proto (100%) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/messages_robocup_ssl_robot_status.proto (100%) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/messages_robocup_ssl_wrapper.proto (100%) rename consai2r2_receiver/{src => include/consai2r2_receiver}/proto/referee.proto (100%) diff --git a/consai2r2_receiver/CMakeLists.txt b/consai2r2_receiver/CMakeLists.txt index 0dc9f5a..6898444 100644 --- a/consai2r2_receiver/CMakeLists.txt +++ b/consai2r2_receiver/CMakeLists.txt @@ -25,13 +25,13 @@ include_directories( ) protobuf_generate_cpp(PROTO_CPP PROTO_H - src/proto/messages_robocup_ssl_wrapper.proto - src/proto/messages_robocup_ssl_geometry.proto - src/proto/messages_robocup_ssl_detection.proto - src/proto/messages_robocup_ssl_robot_status.proto - src/proto/messages_robocup_ssl_refbox_log.proto - src/proto/referee.proto - src/proto/game_event.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_wrapper.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_geometry.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_detection.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_robot_status.proto + include/${PROJECT_NAME}/proto/messages_robocup_ssl_refbox_log.proto + include/${PROJECT_NAME}/proto/referee.proto + include/${PROJECT_NAME}/proto/game_event.proto ) include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/consai2r2_receiver/src/proto/game_event.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto similarity index 100% rename from consai2r2_receiver/src/proto/game_event.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/game_event.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_detection.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_detection.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_geometry.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_geometry.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_refbox_log.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_refbox_log.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_robot_status.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_robot_status.proto diff --git a/consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto similarity index 100% rename from consai2r2_receiver/src/proto/messages_robocup_ssl_wrapper.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/messages_robocup_ssl_wrapper.proto diff --git a/consai2r2_receiver/src/proto/referee.proto b/consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto similarity index 100% rename from consai2r2_receiver/src/proto/referee.proto rename to consai2r2_receiver/include/consai2r2_receiver/proto/referee.proto From 3a94451e3ca487d0a0e16c3cd0e60dc8bcd7499b Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Wed, 18 Dec 2019 21:30:50 +0900 Subject: [PATCH 11/13] delete msgs --- consai2r2_msgs/CMakeLists.txt | 4 --- consai2r2_msgs/msg/BallInfo.msg | 10 ------- consai2r2_msgs/msg/ControlTarget.msg | 21 -------------- consai2r2_msgs/msg/DecodedReferee.msg | 40 --------------------------- consai2r2_msgs/msg/ReplaceBall.msg | 8 ------ consai2r2_msgs/msg/RobotInfo.msg | 29 ------------------- 6 files changed, 112 deletions(-) delete mode 100755 consai2r2_msgs/msg/BallInfo.msg delete mode 100755 consai2r2_msgs/msg/ControlTarget.msg delete mode 100755 consai2r2_msgs/msg/DecodedReferee.msg delete mode 100755 consai2r2_msgs/msg/ReplaceBall.msg delete mode 100755 consai2r2_msgs/msg/RobotInfo.msg diff --git a/consai2r2_msgs/CMakeLists.txt b/consai2r2_msgs/CMakeLists.txt index 330d72f..eefb16b 100644 --- a/consai2r2_msgs/CMakeLists.txt +++ b/consai2r2_msgs/CMakeLists.txt @@ -29,10 +29,6 @@ set(msg_files "msg/DetectionRobot.msg" "msg/DetectionFrame.msg" "msg/VisionDetections.msg" - "msg/RobotInfo.msg" - "msg/BallInfo.msg" - "msg/DecodedReferee.msg" - "msg/ReplaceBall.msg" "msg/RobotCommand.msg" "msg/RobotCommands.msg" ) diff --git a/consai2r2_msgs/msg/BallInfo.msg b/consai2r2_msgs/msg/BallInfo.msg deleted file mode 100755 index 6a268cd..0000000 --- a/consai2r2_msgs/msg/BallInfo.msg +++ /dev/null @@ -1,10 +0,0 @@ -# ball information for consai2_game - -geometry_msgs/Pose2D pose -geometry_msgs/Pose2D velocity -geometry_msgs/Twist velocity_twist -bool detected -builtin_interfaces/Time detection_stamp - -geometry_msgs/Pose2D last_detection_pose -bool disappeared diff --git a/consai2r2_msgs/msg/ControlTarget.msg b/consai2r2_msgs/msg/ControlTarget.msg deleted file mode 100755 index d1a0ff7..0000000 --- a/consai2r2_msgs/msg/ControlTarget.msg +++ /dev/null @@ -1,21 +0,0 @@ -# control target for consai2_game and consai2_control - -uint8 robot_id - -bool control_enable -# path[last] == goal_pose -geometry_msgs/Pose2D[] path -geometry_msgs/Pose2D goal_velocity - -# 0.0 ~ 1.0 -float64 kick_power -# True/False -bool chip_enable -# 0.0 ~ 1.0 -float64 dribble_power - -# default:unused -bool test_flag -# default:unused -float64 test_param - diff --git a/consai2r2_msgs/msg/DecodedReferee.msg b/consai2r2_msgs/msg/DecodedReferee.msg deleted file mode 100755 index 57a6d82..0000000 --- a/consai2r2_msgs/msg/DecodedReferee.msg +++ /dev/null @@ -1,40 +0,0 @@ -# game status for consai2_game - -# ID : text -# 0 : HALT -# 1 : STOP -# 2 : --- -# 3 : FORCE_START -# 11 : OUR_KICKOFF_PREPARATION -# 12 : OUR_KICKOFF_START -# 13 : OUR_PENALTY_PREPARATION -# 14 : OUR_PENALTY_START -# 15 : OUR_DIRECT_FREE -# 16 : OUR_INDIRECT_FREE -# 17 : OUR_TIMEOUT -# 18 : OUR_GOAL -# 19 : OUR_BALL_PLACEMENT -# 20 : --- -# 21 : THEIR_KICKOFF_PREPARATION -# 22 : THEIR_KICKOFF_START -# 23 : THEIR_PENALTY_PREPARATION -# 24 : THEIR_PENALTY_START -# 25 : THEIR_DIRECT_FREE -# 26 : THEIR_INDIRECT_FREE -# 27 : THEIR_TIMEOUT -# 28 : THEIR_GOAL -# 29 : THEIR_BALL_PLACEMENT - -uint8 referee_id -string referee_text - -bool can_move_robot -float64 speed_limit_of_robot # meter/sec -bool can_kick_ball -bool can_enter_their_side -bool can_enter_center_circle -bool is_inplay -float64 keep_out_radius_from_ball # meter -float64 keep_out_distance_from_their_defense_area # meter - -geometry_msgs/Point placement_position diff --git a/consai2r2_msgs/msg/ReplaceBall.msg b/consai2r2_msgs/msg/ReplaceBall.msg deleted file mode 100755 index 546df12..0000000 --- a/consai2r2_msgs/msg/ReplaceBall.msg +++ /dev/null @@ -1,8 +0,0 @@ -# This msg file is copied from 'grSim_Replacement.proto' -# -float64 x -float64 y -float64 vx -float64 vy - -bool is_enabled diff --git a/consai2r2_msgs/msg/RobotInfo.msg b/consai2r2_msgs/msg/RobotInfo.msg deleted file mode 100755 index fa1b49f..0000000 --- a/consai2r2_msgs/msg/RobotInfo.msg +++ /dev/null @@ -1,29 +0,0 @@ -# robot information for consai2_game - -# robot id -uint8 robot_id - -# Robot 2D pose -# This field will be replaced to geometry_msgs/Pose -geometry_msgs/Pose2D pose - -# Robot Velocity -# [Deprecated] This field will be removed. -geometry_msgs/Pose2D velocity - -# Robot Velocity -geometry_msgs/Twist velocity_twist - -# If robot is detected by vision in this frame, this field is set to true -bool detected - -# Latest vision detection time -# [Deprecated] This field will be removed. -builtin_interfaces/Time detection_stamp - -# Latest detected pose. -# This field is NOT filtered. This is a raw vision value. -geometry_msgs/Pose2D last_detection_pose - -# If a robot disappear from field, this field is set to true -bool disappeared From 4b68bcd8f2ad868b7370201d8a7056f0572d1448 Mon Sep 17 00:00:00 2001 From: Mirai Hattori <17918740+future731@users.noreply.github.com> Date: Fri, 20 Dec 2019 15:42:14 +0900 Subject: [PATCH 12/13] fix typo Controbutors->Contributors --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8df1e6e..cedba6e 100644 --- a/README.md +++ b/README.md @@ -109,7 +109,7 @@ issue, pull request 大歓迎です。 consai2r2はRoboCup SSLに参加している日本人チーム**Roots**が立ち上げました。 -Roots以外にもconsai2r2の開発に貢献しているメンバーがいます！[Controbutors](https://github.com/SSL-Roots/consai2r2/graphs/contributors)のページを見てください。 +Roots以外にもconsai2r2の開発に貢献しているメンバーがいます！[Contributors](https://github.com/SSL-Roots/consai2r2/graphs/contributors)のページを見てください。 RoboCup SSLへの参加方法、ロボットに必要な機能、開発環境などは Rootsのホームページに記載してます。 From 91189437403cbf86ce1389af7044da6568791e2c Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sun, 22 Dec 2019 21:16:06 +0900 Subject: [PATCH 13/13] set robot.pose.theta --- consai2r2_receiver/src/vision_receiver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/consai2r2_receiver/src/vision_receiver.cpp b/consai2r2_receiver/src/vision_receiver.cpp index 94f92b6..d74ebf9 100644 --- a/consai2r2_receiver/src/vision_receiver.cpp +++ b/consai2r2_receiver/src/vision_receiver.cpp @@ -94,6 +94,7 @@ class FormatConverter detection_robot.robot_id = raw_robot.robot_id(); detection_robot.pose.x = raw_robot.x() * TO_METER; detection_robot.pose.y = raw_robot.y() * TO_METER; + detection_robot.pose.theta = raw_robot.orientation(); return detection_robot; }