Merge branch 'master' of https://github.com/SSL-Roots/consai2r2 into …

…dev/consai2r2_description

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のホームページに記載してます。
 

consai2r2_msgs/CMakeLists.txt

@@ -13,16 +13,29 @@ 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/FieldCircularArc.msg"
+  "msg/FieldLineSegment.msg"
+  "msg/VisionGeometry.msg"
+  "msg/DetectionBall.msg"
+  "msg/DetectionRobot.msg"
+  "msg/DetectionFrame.msg"
+  "msg/VisionDetections.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)

consai2r2_msgs/msg/DetectionBall.msg

@@ -0,0 +1,3 @@
+# This msg file is copied from 'messages_robocup_ssl_detection.proto'
+
+geometry_msgs/Pose2D pose

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

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

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

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

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

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

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

consai2r2_msgs/msg/VisionDetections.msg

@@ -0,0 +1,2 @@
+std_msgs/Header     header
+DetectionFrame[]    frames

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

consai2r2_msgs/package.xml

@@ -5,12 +5,15 @@
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="macakasit@gmail.com">akshota</maintainer>
-  <license>TODO: License declaration</license>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rosidl_default_generators</depend>
+  <depend>builtin_interfaces</depend>
   <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
 

consai2r2_receiver/CMakeLists.txt

@@ -0,0 +1,85 @@
+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 COMPONENTS system)
+
+# Protobuf
+include_directories(
+  ${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}
+)
+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/
+  DESTINATION include)
+
+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)
+  # 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()

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 <boost/asio.hpp>
+#include <exception>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+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<char> & 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_