diff --git a/camera.bash b/camera.bash new file mode 100644 index 000000000..ab47d063f --- /dev/null +++ b/camera.bash @@ -0,0 +1,10 @@ +device_id = 0 +v4l2-ctl -d $device_id -c exposure_dynamic_framerate=0 +v4l2-ctl -d $device_id -c auto_exposure=1 +v4l2-ctl -d $device_id -c focus_automatic_continuous=0 +v4l2-ctl -d $device_id -c focus_absolute=0 + + +v4l2-ctl -d $device_id -c exposure_time_absolute=120 +v4l2-ctl -d $device_id -c brightness=120 +v4l2-ctl -d $device_id -c sharpness=400 diff --git a/consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py b/consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py index 71796f74c..6a3ce39f3 100644 --- a/consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py +++ b/consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py @@ -14,21 +14,23 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math from enum import Enum +import math -from python_qt_binding.QtCore import QPoint, QPointF, QRect, QSize, Qt -from python_qt_binding.QtGui import QColor, QPainter, QPen +from python_qt_binding.QtCore import QPointF +from python_qt_binding.QtCore import QRectF +from python_qt_binding.QtCore import QSizeF +from python_qt_binding.QtCore import Qt +from python_qt_binding.QtGui import QColor +from python_qt_binding.QtGui import QPainter +from python_qt_binding.QtGui import QPen from python_qt_binding.QtWidgets import QWidget - -# from robocup_ssl_msgs.msg import Point as RefPoint -from robocup_ssl_msgs.msg import ( - BallReplacement, - GeometryFieldSize, - Replacement, - RobotId, - TrackedFrame, -) +from robocup_ssl_msgs.msg import BallReplacement +from robocup_ssl_msgs.msg import GeometryFieldSize +from robocup_ssl_msgs.msg import Replacement +from robocup_ssl_msgs.msg import RobotId +from robocup_ssl_msgs.msg import RobotReplacement +from robocup_ssl_msgs.msg import TrackedFrame class ClickedObject(Enum): @@ -38,6 +40,7 @@ class ClickedObject(Enum): class FieldWidget(QWidget): + def __init__(self, parent=None): super(FieldWidget, self).__init__(parent) @@ -45,13 +48,12 @@ def __init__(self, parent=None): # Ref: named color https://www.w3.org/TR/SVG11/types.html#ColorKeywords self._COLOR_FIELD_CARPET = Qt.green self._COLOR_FIELD_LINE = Qt.white - self._COLOR_BALL = QColor("orange") + self._COLOR_BALL = QColor('orange') self._COLOR_BLUE_ROBOT = Qt.cyan self._COLOR_YELLOW_ROBOT = Qt.yellow - self._COLOR_REPLACEMENT_POS = QColor("magenta") - self._COLOR_REPLACEMENT_VEL_ANGLE = QColor("darkviolet") - self._COLOR_GOAL_POSE = QColor("silver") - self._COLOR_DESIGNATED_POSITION = QColor("red") + self._COLOR_REPLACEMENT_POS = QColor('magenta') + self._COLOR_REPLACEMENT_VEL_ANGLE = QColor('darkviolet') + self._COLOR_DESIGNATED_POSITION = QColor('red') self._THICKNESS_FIELD_LINE = 2 self._MOUSE_WHEEL_ZOOM_RATE = 0.2 # マウスホイール操作による拡大縮小操作量 self._LIMIT_SCALE = 0.2 # 縮小率の限界値 @@ -65,7 +67,7 @@ def __init__(self, parent=None): self._RADIUS_REPLACEMENT_ROBOT_ANGLE = self._RADIUS_ROBOT + 200 self._GAIN_REPLACE_BALL_VEL = 0.001 * 3.0 self._MAX_VELOCITY_OF_REPLACE_BALL = 8.0 - self._ID_POS = QPoint(150, 150) # IDの表示位置 mm. + self._ID_POS = QPointF(150.0, 150.0) # IDの表示位置 mm. self._RADIUS_DESIGNATED_POSITION = 150 # ボールプレースメント成功範囲 # 外部からセットするパラメータ @@ -80,14 +82,19 @@ def __init__(self, parent=None): self._field.field_width = 9000 # resize_draw_area()で0 divisionを防ぐための初期値 self._detections = {} self._detection_tracked = TrackedFrame() + self._blue_robot_num = 0 + self._yellow_robot_num = 0 # self._goal_poses = {} self._robot_commands = {} + self._final_goal_poses = {} self._designated_position = {} # ball placementの目標位置 + self._named_targets = {} + self._robot_replacements = [] # 内部で変更するパラメータ self._draw_area_scale = 1.0 # 描画領域の拡大縮小率 self._draw_area_offset = QPointF(0.0, 0.0) # 描画領域のオフセット - self._draw_area_size = self.rect().size() # 描画領域サイズ + self._draw_area_size = QSizeF(self.rect().size()) # 描画領域サイズ self._scale_field_to_draw = 1.0 # フィールド領域から描画領域に縮小するスケール self._do_rotate_draw_area = False # 描画領域を90度回転するフラグ self._mouse_clicked_point = QPointF(0.0, 0.0) # マウスでクリックした描画領域の座標 @@ -122,6 +129,7 @@ def set_detection(self, msg): def set_detection_tracked(self, msg): self._detection_tracked = msg + self._update_robot_num() def set_robot_command(self, msg, robot_id, is_yellow: bool): self._robot_commands[robot_id] = msg @@ -130,6 +138,33 @@ def set_robot_command(self, msg, robot_id, is_yellow: bool): def set_designated_position(self, msg): self._designated_position[0] = msg + def set_named_targets(self, msg): + # トピックを受け取るたびに初期化する + self._named_targets = {} + for i in range(len(msg.name)): + self._named_targets[msg.name[i]] = msg.pose[i] + + def get_blue_robot_num(self): + return self._blue_robot_num + + def get_yellow_robot_num(self): + return self._yellow_robot_num + + def append_robot_replacement(self, is_yellow, robot_id, turnon): + # turnon時はフィールド内に、turnoff時はフィールド外に、ID順でロボットを並べる + ID_OFFSET_X = 0.2 + team_offset_x = -3.0 if is_yellow else 0.0 + turnon_offset_y = -4.6 if turnon else -5.6 + + replacement = RobotReplacement() + replacement.x = team_offset_x + ID_OFFSET_X * robot_id + replacement.y = turnon_offset_y + replacement.dir = 90.0 + replacement.id = robot_id + replacement.yellowteam = is_yellow + replacement.turnon.append(turnon) + self._robot_replacements.append(replacement) + def reset_topics(self): # 取得したトピックをリセットする self._detections = {} @@ -146,8 +181,7 @@ def mousePressEvent(self, event): # ロボット、ボールをクリックしているか if self._can_draw_replacement: self._clicked_replacement_object = self._get_clicked_replacement_object( - self._mouse_clicked_point - ) + self._mouse_clicked_point) elif event.buttons() == Qt.RightButton: self._reset_draw_area_offset_and_scale() @@ -164,8 +198,7 @@ def mouseMoveEvent(self, event): elif event.buttons() == Qt.LeftButton: # 描画領域を移動するためのオフセットを計算 self._mouse_drag_offset = ( - self._mouse_current_point - self._mouse_clicked_point - ) / self._draw_area_scale + self._mouse_current_point - self._mouse_clicked_point) / self._draw_area_scale self.update() @@ -217,6 +250,10 @@ def paintEvent(self, event): if self._can_draw_geometry: self._draw_geometry(painter) + # 名前付きターゲットを描画 + self._draw_named_targets(painter) + + # ボールプレースメントでの目標位置と進入禁止エリアを描画 self._draw_designated_position(painter) if self._can_draw_replacement: @@ -228,7 +265,12 @@ def paintEvent(self, event): if self._can_draw_detection_tracked: self._draw_detection_tracked(painter) - painter.end() + # ロボットのreplacementをpublishする + if self._robot_replacements: + replacement = Replacement() + replacement.robots = self._robot_replacements + self._pub_replacement.publish(replacement) + self._robot_replacements = [] def _get_clicked_replacement_object(self, clicked_point): # マウスでクリックした位置がボールやロボットに近いか判定する @@ -243,7 +285,7 @@ def _get_clicked_replacement_object(self, clicked_point): return clicked_object def _get_clicked_ball_object(self, clicked_pos, ball): - ball_pos = QPoint(ball.x, ball.y) + ball_pos = QPointF(ball.x, ball.y) if self._is_clicked(ball_pos, clicked_pos, self._RADIUS_REPLACEMENT_BALL_POS): return ClickedObject.IS_BALL_POS @@ -268,6 +310,8 @@ def _publish_ball_pos_replacement(self): pos = self._convert_draw_to_field_pos(self._mouse_current_point) ball_replacement.x.append(pos.x() * 0.001) # mm に変換 ball_replacement.y.append(pos.y() * 0.001) # mm に変換 + ball_replacement.vx.append(0.0) + ball_replacement.vy.append(0.0) replacement = Replacement() replacement.ball.append(ball_replacement) self._pub_replacement.publish(replacement) @@ -297,6 +341,24 @@ def _publish_ball_vel_replacement(self): replacement.ball.append(ball_replacement) self._pub_replacement.publish(replacement) + def _update_robot_num(self): + # 存在するロボットの台数を計上する + blue_robot_num = 0 + yellow_robot_num = 0 + for robot in self._detection_tracked.robots: + if len(robot.visibility) <= 0: + continue + + if robot.visibility[0] < 0.01: + continue + + if robot.robot_id.team_color == RobotId.TEAM_COLOR_YELLOW: + yellow_robot_num += 1 + else: + blue_robot_num += 1 + self._blue_robot_num = blue_robot_num + self._yellow_robot_num = yellow_robot_num + def _reset_draw_area_offset_and_scale(self): # 描画領域の移動と拡大・縮小を初期化する self._draw_area_offset = QPointF(0.0, 0.0) @@ -321,20 +383,20 @@ def _resize_draw_area(self): # Widgetのサイズによって描画領域を回転するか判定する if widget_w_per_h >= field_w_per_h: # Widgetが横長のとき - self._draw_area_size = QSize(int(widget_height * field_w_per_h), int(widget_height)) + self._draw_area_size = QSizeF(widget_height * field_w_per_h, widget_height) self._do_rotate_draw_area = False elif widget_w_per_h <= field_h_per_w: # Widgetが縦長のとき - self._draw_area_size = QSize(int(widget_width * field_w_per_h), int(widget_width)) + self._draw_area_size = QSizeF(widget_width * field_w_per_h, widget_width) self._do_rotate_draw_area = True else: # 描画回転にヒステリシスをもたせる if self._do_rotate_draw_area is True: - self._draw_area_size = QSize(int(widget_height), int(widget_height * field_h_per_w)) + self._draw_area_size = QSizeF(widget_height, widget_height * field_h_per_w) else: - self._draw_area_size = QSize(int(widget_width), int(widget_width * field_h_per_w)) + self._draw_area_size = QSizeF(widget_width, widget_width * field_h_per_w) self._scale_field_to_draw = self._draw_area_size.width() / field_full_width @@ -343,11 +405,9 @@ def _draw_geometry(self, painter): # グリーンカーペットを描画 painter.setBrush(self._COLOR_FIELD_CARPET) - rect = QRect( - QPoint( - int(-self._draw_area_size.width() * 0.5), int(-self._draw_area_size.height() * 0.5) - ), - self._draw_area_size, + rect = QRectF( + QPointF(-self._draw_area_size.width() * 0.5, -self._draw_area_size.height() * 0.5), + self._draw_area_size ) painter.drawRect(rect) @@ -360,42 +420,43 @@ def _draw_geometry(self, painter): for arc in self._field.field_arcs: top_left = self._convert_field_to_draw_point( - arc.center.x - arc.radius, arc.center.y + arc.radius - ) - size = int(arc.radius * 2 * self._scale_field_to_draw) + arc.center.x - arc.radius, + arc.center.y + arc.radius) + size = arc.radius * 2 * self._scale_field_to_draw + rect = QRectF(top_left, QSizeF(size, size)) # angle must be 1/16 degrees order - start_angle = math.degrees(arc.a1) * 16 - end_angle = math.degrees(arc.a2) * 16 + start_angle = int(math.degrees(arc.a1) * 16) + end_angle = int(math.degrees(arc.a2) * 16) span_angle = end_angle - start_angle - painter.drawArc( - int(top_left.x()), int(top_left.y()), size, size, int(start_angle), int(span_angle) - ) + painter.drawArc(rect, start_angle, span_angle) + + # ペナルティポイントを描画 + painter.setBrush(self._COLOR_FIELD_LINE) + PENALTY_X = self._field.field_length * 0.5 - 8000 # ルールの2.1.3 Field Markingsを参照 + PENALTY_DRAW_SIZE = 50 * self._scale_field_to_draw + point = self._convert_field_to_draw_point(PENALTY_X, 0.0) + painter.drawEllipse(point, PENALTY_DRAW_SIZE, PENALTY_DRAW_SIZE) + point = self._convert_field_to_draw_point(-PENALTY_X, 0.0) + painter.drawEllipse(point, PENALTY_DRAW_SIZE, PENALTY_DRAW_SIZE) # ゴールを描画 painter.setPen(QPen(Qt.black, self._THICKNESS_FIELD_LINE)) - rect = QRect( + painter.setBrush(self._COLOR_FIELD_CARPET) + rect = QRectF( self._convert_field_to_draw_point( - self._field.field_length * 0.5, self._field.goal_width * 0.5 - ), - QSize( - int(self._field.goal_depth * self._scale_field_to_draw), - int(self._field.goal_width * self._scale_field_to_draw), - ), - ) + self._field.field_length * 0.5, self._field.goal_width*0.5), + QSizeF(self._field.goal_depth * self._scale_field_to_draw, + self._field.goal_width * self._scale_field_to_draw)) painter.drawRect(rect) painter.setPen(QPen(Qt.black, self._THICKNESS_FIELD_LINE)) - rect = QRect( + rect = QRectF( self._convert_field_to_draw_point( -self._field.field_length * 0.5 - self._field.goal_depth, - self._field.goal_width * 0.5, - ), - QSize( - int(self._field.goal_depth * self._scale_field_to_draw), - int(self._field.goal_width * self._scale_field_to_draw), - ), - ) + self._field.goal_width*0.5), + QSizeF(self._field.goal_depth * self._scale_field_to_draw, + self._field.goal_width * self._scale_field_to_draw)) painter.drawRect(rect) def _draw_detection(self, painter): @@ -453,8 +514,7 @@ def _draw_tracked_ball(self, painter, ball): painter.setPen(color_pen) painter.setBrush(color_brush) point_pos = self._convert_field_to_draw_point( - ball.pos.x * 1000, ball.pos.y * 1000 - ) # meters to mm + ball.pos.x * 1000, ball.pos.y * 1000) # meters to mm size = self._RADIUS_BALL * self._scale_field_to_draw painter.drawEllipse(point_pos, size, size) @@ -470,8 +530,7 @@ def _draw_tracked_ball(self, painter, ball): vel_end_x = PAINT_LENGTH * math.cos(direction) + ball.pos.x vel_end_y = PAINT_LENGTH * math.sin(direction) + ball.pos.y point_vel = self._convert_field_to_draw_point( - vel_end_x * 1000, vel_end_y * 1000 - ) # meters to mm + vel_end_x * 1000, vel_end_y * 1000) # meters to mm painter.setPen(QPen(QColor(102, 0, 255), 2)) painter.drawLine(point_pos, point_vel) @@ -503,9 +562,8 @@ def _draw_detection_robot(self, painter, robot, color, camera_id=-1): # ロボットID if len(robot.robot_id) > 0: - text_point = point + self._convert_field_to_draw_point( - self._ID_POS.x(), self._ID_POS.y() - ) + text_point = point + \ + self._convert_field_to_draw_point(self._ID_POS.x(), self._ID_POS.y()) painter.drawText(text_point, str(robot.robot_id[0])) def _draw_tracked_robot(self, painter, robot): @@ -532,8 +590,7 @@ def _draw_tracked_robot(self, painter, robot): painter.setPen(color_pen) painter.setBrush(color_brush) point = self._convert_field_to_draw_point( - robot.pos.x * 1000, robot.pos.y * 1000 - ) # meters to mm + robot.pos.x * 1000, robot.pos.y * 1000) # meters to mm size = self._RADIUS_ROBOT * self._scale_field_to_draw painter.drawEllipse(point, size, size) @@ -563,8 +620,25 @@ def _draw_goal_pose(self, painter, robot): if robot_is_yellow is not self.team_is_yellow: return + brush_color = QColor('silver') + line_color = QColor('red') + self._draw_goal_pose_and_line_to_parent( + painter, robot_command, robot.pos.x, robot.pos.y, robot_id, + self._RADIUS_ROBOT * self._scale_field_to_draw, brush_color, line_color) + + brush_color.setAlphaF(0.5) + line_color.setAlphaF(0.5) + self._draw_goal_pose_and_line_to_parent( + painter, robot_command, robot_command.target_x[0], robot_command.target_y[0], robot_id, + self._RADIUS_ROBOT * 0.7 * self._scale_field_to_draw, brush_color, line_color) + + def _draw_goal_pose_and_line_to_parent( + self, painter, robot_command, parent_x, parent_y, robot_id, + size, color_brush, color_line_to_parent): + # 目標位置を描画する + # parentはロボットだったり、回避位置だったりする painter.setPen(Qt.black) - painter.setBrush(self._COLOR_GOAL_POSE) + painter.setBrush(color_brush) # x,y座標 point = self._convert_field_to_draw_point( robot_command.target_x[0] * 1000, robot_command.target_y[0] * 1000 @@ -586,11 +660,10 @@ def _draw_goal_pose(self, painter, robot): painter.drawText(text_point, str(robot_id)) # goal_poseとロボットを結ぶ直線を引く - painter.setPen(QPen(Qt.red, 2)) - robot_point = self._convert_field_to_draw_point( - robot.pos.x * 1000, robot.pos.y * 1000 - ) # meters to mm - painter.drawLine(point, robot_point) + painter.setPen(QPen(color_line_to_parent, 2)) + parent_point = self._convert_field_to_draw_point( + parent_x * 1000, parent_y * 1000) # meters to mm + painter.drawLine(point, parent_point) def _draw_velocity(self, painter, robot): # ロボットの情報も参照するため、draw_tracked_robot()から呼び出すこと @@ -665,21 +738,77 @@ def _draw_replacement_ball(self, painter, ball): painter.drawLine(point, end_point) def _draw_designated_position(self, painter): - if self._designated_position: - point = self._convert_field_to_draw_point( - self._designated_position[0].x, self._designated_position[0].y - ) - size = self._RADIUS_DESIGNATED_POSITION * self._scale_field_to_draw + # ボールプレースメントの目標位置と進入禁止エリアを描画する関数 + if not self._designated_position: + return + if not self._detection_tracked.balls: + return + + # 目標位置とボール位置を取得 + designated_point = self._convert_field_to_draw_point( + self._designated_position[0].x, self._designated_position[0].y) + + ball = self._detection_tracked.balls[0] + ball_point = self._convert_field_to_draw_point( + ball.pos.x * 1000, ball.pos.y * 1000) # meters to mm + + # ボール配置しないロボットは、ボールと目標位置を結ぶラインから0.5 m以上離れなければならない + PROHIBITED_DISTANCE = 500 + color_prohibited = QColor(Qt.black) + color_prohibited.setAlpha(100) + painter.setPen(color_prohibited) + painter.setBrush(color_prohibited) + size = PROHIBITED_DISTANCE * self._scale_field_to_draw + + # 目標位置周りの進入禁止エリアを描画 + painter.drawEllipse(designated_point, size, size) + + # ボール周りの進入禁止エリアを描画 + painter.drawEllipse(ball_point, size, size) + + # ボールと目標位置を結ぶラインを描画 + pen_line = QPen() + pen_line.setColor(color_prohibited) + pen_line.setWidthF(size * 2.0) # 幅を2倍にする + painter.setPen(pen_line) + painter.drawLine(designated_point, ball_point) + + # 目標位置を描画 + size = self._RADIUS_DESIGNATED_POSITION * self._scale_field_to_draw + painter.setPen(self._COLOR_DESIGNATED_POSITION) + painter.setBrush(self._COLOR_DESIGNATED_POSITION) + painter.drawEllipse(designated_point, size, size) + + def _draw_named_targets(self, painter): + # 名前付きターゲットを描画する + TARGET_RADIUS = 70 # ターゲット位置の描画直径 mm + NAME_POS = QPointF(100.0, 100.0) # ターゲット名の描画座標 mm - painter.setPen(self._COLOR_DESIGNATED_POSITION) - painter.setBrush(self._COLOR_DESIGNATED_POSITION) + painter.setPen(Qt.black) + painter.setBrush(Qt.white) + + for name, pose in self._named_targets.items(): + # x,y座標 + point = self._convert_field_to_draw_point( + pose.x * 1000, pose.y * 1000) # meters to mm + size = TARGET_RADIUS * self._scale_field_to_draw painter.drawEllipse(point, size, size) + # 角度 + line_x = TARGET_RADIUS * math.cos(pose.theta) + line_y = TARGET_RADIUS * math.sin(pose.theta) + line_point = point + self._convert_field_to_draw_point(line_x, line_y) + painter.drawLine(point, line_point) + + # 名前 + text_point = point + self._convert_field_to_draw_point(NAME_POS.x(), NAME_POS.y()) + painter.drawText(text_point, str(name)) + def _convert_field_to_draw_point(self, x, y): # フィールド座標系を描画座標系に変換する - draw_x = int(x * self._scale_field_to_draw) - draw_y = int(-y * self._scale_field_to_draw) - point = QPoint(draw_x, draw_y) + draw_x = x * self._scale_field_to_draw + draw_y = -y * self._scale_field_to_draw + point = QPointF(draw_x, draw_y) return point def _convert_draw_to_field_pos(self, point): @@ -704,13 +833,13 @@ def _convert_draw_to_field_pos(self, point): field_x /= self._scale_field_to_draw field_y /= -self._scale_field_to_draw - return QPoint(int(field_x), int(field_y)) + return QPointF(field_x, field_y) def _apply_transpose_to_draw_point(self, point): # Widget上のポイント（左上が0, 0となる座標系）を、 # translate、scale、rotate適用後のポイントに変換する - draw_point = QPoint() + draw_point = QPointF() # 描画中心座標変更の適用 draw_point.setX(point.x() - self.width() * 0.5) @@ -724,6 +853,6 @@ def _apply_transpose_to_draw_point(self, point): # 描画領域の回転の適用 if self._do_rotate_draw_area is True: - draw_point = QPoint(int(-draw_point.y()), int(draw_point.x())) + draw_point = QPointF(-draw_point.y(), draw_point.x()) return draw_point diff --git a/consai_ros2/robocup_ssl_comm/src/grsim_component.cpp b/consai_ros2/robocup_ssl_comm/src/grsim_component.cpp index 6c212df09..351f1a297 100644 --- a/consai_ros2/robocup_ssl_comm/src/grsim_component.cpp +++ b/consai_ros2/robocup_ssl_comm/src/grsim_component.cpp @@ -35,12 +35,8 @@ GrSim::GrSim(const rclcpp::NodeOptions & options) : Node("grsim", options) create_subscription("commands", 10, std::bind(&GrSim::callback_commands, this, _1)); sub_replacement = create_subscription( "replacement", 10, std::bind(&GrSim::callback_replacement, this, _1)); - - // timer_ = create_wall_timer(1s, std::bind(&GrSim::on_timer, this)); } -void GrSim::on_timer() { RCLCPP_INFO(get_logger(), "Hello World!"); } - void GrSim::callback_commands(const Commands::SharedPtr msg) { grSim_Commands * commands = new grSim_Commands(); diff --git a/crane_bringup/launch/play_switcher.launch.py b/crane_bringup/launch/play_switcher.launch.py index ea8cf02cf..58e877fe4 100755 --- a/crane_bringup/launch/play_switcher.launch.py +++ b/crane_bringup/launch/play_switcher.launch.py @@ -112,7 +112,9 @@ def generate_launch_description(): parameters=[ { "enable_rvo": False, - "non_rvo_gain": 2.15, + # "non_rvo_gain": 2.15, + "non_rvo_p_gain": 1.0, + "non_rvo_d_gain": 1.0, } ], ) @@ -182,7 +184,7 @@ def generate_launch_description(): real_sender = Node( package="crane_sender", executable="real_sender_node", - # output="screen", + output="screen", parameters=[{}], ) diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 3851d384f..e133f90a5 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -21,6 +21,38 @@ namespace crane { class LocalPlannerComponent : public rclcpp::Node { + class PIDController + { + public: + PIDController() = default; + + void setGain(float kp, float ki, float kd) + { + this->kp = kp; + this->ki = ki; + this->kd = kd; + error_prev = 0.0f; + } + + float update(float error, float dt) + { + float p = kp * error; + float i = ki * (error + error_prev) * dt / 2.0f; + float d = kd * (error - error_prev) / dt; + error_prev = error; + return p + i + d; + } + + private: + float kp; + + float ki; + + float kd; + + float error_prev; + }; + public: COMPOSITION_PUBLIC explicit LocalPlannerComponent(const rclcpp::NodeOptions & options) @@ -51,8 +83,21 @@ class LocalPlannerComponent : public rclcpp::Node RVO_TRAPEZOIDAL_MAX_SPEED = get_parameter("rvo_trapezoidal_max_speed").as_double(); declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL); NON_RVO_MAX_VEL = get_parameter("non_rvo_max_vel").as_double(); - declare_parameter("non_rvo_gain", NON_RVO_GAIN); - NON_RVO_GAIN = get_parameter("non_rvo_gain").as_double(); + + declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN); + NON_RVO_P_GAIN = get_parameter("non_rvo_p_gain").as_double(); + declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN); + NON_RVO_I_GAIN = get_parameter("non_rvo_i_gain").as_double(); + declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN); + NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double(); + + for (auto & controller : vx_controllers) { + controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); + } + + for (auto & controller : vy_controllers) { + controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); + } rvo_sim = std::make_unique( RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, @@ -94,6 +139,9 @@ class LocalPlannerComponent : public rclcpp::Node bool enable_rvo; + std::array vx_controllers; + std::array vy_controllers; + float RVO_TIME_STEP = 1.0 / 60.0f; float RVO_NEIGHBOR_DIST = 2.0f; int RVO_MAX_NEIGHBORS = 5; @@ -107,7 +155,9 @@ class LocalPlannerComponent : public rclcpp::Node float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0; double NON_RVO_MAX_VEL = 4.0; - double NON_RVO_GAIN = 4.0; + double NON_RVO_P_GAIN = 4.0; + double NON_RVO_I_GAIN = 0.0; + double NON_RVO_D_GAIN = 0.0; }; } // namespace crane diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index f5639a9dc..e9ae64516 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -221,9 +221,10 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo // 速度に変換する Velocity vel; - vel << command.target_x.front() - command.current_pose.x, - command.target_y.front() - command.current_pose.y; - vel *= NON_RVO_GAIN; + vel << vx_controllers[command.robot_id].update( + command.target_x.front() - command.current_pose.x, 1.f / 30.f), + vy_controllers[command.robot_id].update( + command.target_y.front() - command.current_pose.y, 1.f / 30.f); vel += vel.normalized() * command.local_planner_config.terminal_velocity; if (vel.norm() > max_vel) { vel = vel.normalized() * max_vel; @@ -232,17 +233,19 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo command.target_velocity.x = vel.x(); command.target_velocity.y = vel.y(); - double MAX_THETA_DIFF = max_omega / 30.0f; - // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する - if (not command.target_theta.empty()) { - double theta_diff = - getAngleDiff(command.target_theta.front(), command.current_pose.theta); - if (std::fabs(theta_diff) > MAX_THETA_DIFF) { - theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff); - } - - command.target_theta.front() = command.current_pose.theta + theta_diff; - } + //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする + // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P. + // double MAX_THETA_DIFF = max_omega / 30.0f; + // // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する + // if (not command.target_theta.empty()) { + // double theta_diff = + // getAngleDiff(command.target_theta.front(), command.current_pose.theta); + // if (std::fabs(theta_diff) > MAX_THETA_DIFF) { + // theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff); + // } + // + // command.target_theta.front() = command.current_pose.theta + theta_diff; + // } command.current_ball_x = world_model->ball.pos.x(); command.current_ball_y = world_model->ball.pos.y(); diff --git a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp index 3e3298656..8403f64d8 100644 --- a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp +++ b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp @@ -78,7 +78,7 @@ class TurnAroundPoint : public SkillBase<> double max_turn_omega = M_PI_4; - double max_velocity = 0.1; + double max_velocity = 0.5; }; } // namespace crane #endif // CRANE_ROBOT_SKILLS__TURN_AROUND_POINT_HPP_ diff --git a/crane_sender/package.xml b/crane_sender/package.xml index ff4cfb911..eebbe6faf 100755 --- a/crane_sender/package.xml +++ b/crane_sender/package.xml @@ -15,6 +15,7 @@ robocup_ssl_msgs sensor_msgs std_msgs + crane_msg_wrappers ament_lint_auto crane_lint_common diff --git a/crane_sender/src/real_sender_node.cpp b/crane_sender/src/real_sender_node.cpp index 9ed917179..516fa79e3 100644 --- a/crane_sender/src/real_sender_node.cpp +++ b/crane_sender/src/real_sender_node.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -39,9 +40,13 @@ class RealSenderNode : public SenderBase { private: int debug_id; + std::shared_ptr parameter_subscriber; + std::shared_ptr parameter_callback_handle; + WorldModelWrapper::SharedPtr world_model; + public: CLASS_LOADER_PUBLIC explicit RealSenderNode(const rclcpp::NodeOptions & options) : SenderBase("real_sender", options) @@ -58,10 +63,16 @@ class RealSenderNode : public SenderBase } }); + world_model = std::make_shared(*this); + std::cout << "start" << std::endl; } + void sendCommands(const crane_msgs::msg::RobotCommands & msg) override { + if (not world_model->hasUpdated()) { + return; + } uint8_t send_packet[32] = {}; constexpr double MAX_VEL_SURGE = 7.0; // m/s @@ -168,20 +179,6 @@ class RealSenderNode : public SenderBase } }(); - // キーパーEN - // 0 or 1 - uint8_t keeper_EN = command.local_goalie_enable; - - // Vision位置 - // -32.767 ~ 0 ~ 32.767 -> 0 ~ 32767 ~ 65534 - // meter -> mili meter - auto [vision_x_low, vision_x_high] = to_two_byte(command.current_pose.x, 32.767); - auto [vision_y_low, vision_y_high] = to_two_byte(command.current_pose.y, 32.767); - - //ボール座標 - auto [ball_x_low, ball_x_high] = to_two_byte(command.current_ball_x, 32.767); - auto [ball_y_low, ball_y_high] = to_two_byte(command.current_ball_y, 32.767); - // 目標座標 float target_x = 0.f; float target_y = 0.f; @@ -196,6 +193,33 @@ class RealSenderNode : public SenderBase } else { enable_local_feedback = false; } + + enable_local_feedback = false; + + std::vector available_ids = world_model->ours.getAvailableRobotIds(); + bool is_id_available = + std::count(available_ids.begin(), available_ids.end(), command.robot_id) == 1; + std::cout << "id( " << command.robot_id << " ) is available: " << is_id_available + << std::endl; + // キーパーEN + // 0 or 1 + + uint8_t keeper_EN = command.local_goalie_enable; + uint8_t local_flags = 0x00; + local_flags |= (is_id_available << 0); + local_flags |= (enable_local_feedback << 2); + local_flags |= (keeper_EN << 4); + + // Vision位置 + // -32.767 ~ 0 ~ 32.767 -> 0 ~ 32767 ~ 65534 + // meter -> mili meter + auto [vision_x_low, vision_x_high] = to_two_byte(command.current_pose.x, 32.767); + auto [vision_y_low, vision_y_high] = to_two_byte(command.current_pose.y, 32.767); + + //ボール座標 + auto [ball_x_low, ball_x_high] = to_two_byte(command.current_ball_x, 32.767); + auto [ball_y_low, ball_y_high] = to_two_byte(command.current_ball_y, 32.767); + auto [target_x_low, target_x_high] = to_two_byte(target_x, 32.767); auto [target_y_low, target_y_high] = to_two_byte(target_y, 32.767); @@ -217,7 +241,7 @@ class RealSenderNode : public SenderBase send_packet[8] = static_cast(target_theta_low); send_packet[9] = static_cast(kick_power_send); send_packet[10] = static_cast(dribble_power_send); - send_packet[11] = static_cast(keeper_EN); + send_packet[11] = static_cast(local_flags); send_packet[12] = static_cast(ball_x_high); send_packet[13] = static_cast(ball_x_low); send_packet[14] = static_cast(ball_y_high); @@ -230,7 +254,6 @@ class RealSenderNode : public SenderBase send_packet[21] = static_cast(target_x_low); send_packet[22] = static_cast(target_y_high); send_packet[23] = static_cast(target_y_low); - send_packet[24] = static_cast(enable_local_feedback); if (command.robot_id == debug_id) { printf( diff --git a/docs/grSim.md b/docs/grSim.md new file mode 100644 index 000000000..2299ccaae --- /dev/null +++ b/docs/grSim.md @@ -0,0 +1,3 @@ +# grSim + + diff --git a/docs/network.md b/docs/network.md new file mode 100644 index 000000000..0ef0b870c --- /dev/null +++ b/docs/network.md @@ -0,0 +1,32 @@ +# ネットワーク設定 + +## ROS関連 + +https://autowarefoundation.github.io/autoware-documentation/pr-347/installation/additional-settings-for-developers/#network-settings-for-ros-2 + +### ローカルホストでマルチキャスト +```bash +sudo ip link set multicast on lo +``` + +## インターネット接続とロボット接続の共存 + +ロボットのアドレスに対して静的ルーティングを設定する + +`/etc/netplan/01-network-manager-all.yaml` +```yaml +network: + version: 2 + renderer: networkd + ethernets: + enp3s0: + addresses: + - 192.168.1.2/24 + gateway4: 192.168.1.1 + routes: + - to: 192.168.2.0/24 + via: 192.168.1.1 + - to: 192.168.3.0/24 + via: 192.168.1.1 + +``` diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp index 0deb51ded..1f8573a95 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp @@ -92,6 +92,8 @@ class BallPlacementWithSkillPlanner : public PlannerBase state = BallPlacementState::GET_BALL_CONTACT; turn_around_point = nullptr; } + command.setMaxVelocity(0.5); + command.setTerminalVelocity(0.5); } else if (state == BallPlacementState::GET_BALL_CONTACT) { if (get_ball_contact->run(command) == SkillBase<>::Status::SUCCESS) { move_with_ball->target_pose.pos = diff --git a/session/crane_session_controller/config/defense.yaml b/session/crane_session_controller/config/defense.yaml new file mode 100644 index 000000000..9d058a233 --- /dev/null +++ b/session/crane_session_controller/config/defense.yaml @@ -0,0 +1,18 @@ +--- +events: + - event: "HALT" + session: "HALT" + - event: "STOP" + session: "formation" + - event: "THEIR_KICKOFF_PREPARATION" + session: "formation" + - event: "THEIR_KICKOFF_START" + session: "formation" + - event: "OUR_KICKOFF_PREPARATION" + session: "formation" + - event: "OUR_KICKOFF_START" + session: "SIMPLE_ATTACK" + - event: "INPLAY" + session: "SIMPLE_ATTACK" + - event: "OUR_BALL_PLACEMENT" + session: "ball_placement_with_skill"