Skip to content

hawking-npu/gazebo_ws

Repository files navigation

simatch

Package Summary

Overview

Note: This is for SIMULATION. The following packages should be used together:

However, if you find this cumbersome or difficult, you could also refer to this repository "simatch" which is an optimized combination of these packages with simple user configuration. Although the repository "simatch" is used directly for the China Robotics Competition Middle Size Simulation League, which aims to promote the development of multi-robot coordination research, it could also be utilized for other research purposes.

package description
gazebo_visual For gazebo visulization
nubot_ws For real robot code
coach4sim For sending game command such as kickoff to robots; this does not require rtdb

Kind remind
If you want to use coach4sim, you need to install Qt. For those who don't want to install Qt, a solution is using command line to send game commands as follows:
In another terminal, input the following to send a game command:

rostopic pub -r 1 /nubot/receive_from_coach  nubot_common/CoachInfo "
MatchMode: 10
MatchType: 0" 

Indeed, when you input until nubot_common/CoachInfo, you could press 'Tab' twice and then the whole definition of the message would show up. Then you could fill up the message. However, you only need to fill in two fields: 'MatchMode' and 'MatchType', where 'MatchMode' is the current game command, 'MatchType' is the previous game command. The coding of the game commands is in core.hpp. For quick reference:

enum MatchMode {
                  STOPROBOT  =  0,
                  OUR_KICKOFF = 1,
                  OPP_KICKOFF = 2,
                  OUR_THROWIN = 3,
                  OPP_THROWIN = 4,
                  OUR_PENALTY = 5,
                  OPP_PENALTY = 6,
                  OUR_GOALKICK = 7 ,
                  OPP_GOALKICK = 8,
                  OUR_CORNERKICK = 9,
                  OPP_CORNERKICK = 10,
                  OUR_FREEKICK = 11,
                  OPP_FREEKICK = 12,
                  DROPBALL     = 13,
                  STARTROBOT   = 15,
                  PARKINGROBOT = 25,
                  TEST = 27
                };

The robot movement is realized by a Gazebo model plugin which is called "NubotGazebo" generated by source files "nubot_gazebo.cc" and "nubot_gazebo.hh". Basically the essential part of the plugin is realizing basic motions: omnidirectional locomotion, ball-dribbling and ball-kicking.

The gazebo plugin(single_nubot_gazebo) subscribes to topic "nubotcontrol/velcmd" for omnidirecitonal movement and subscribes to service "BallHandle" and "Shoot" for ball-dribbling and ball-kicking respectively. For this package(gazebo_visual), there is a new topic named "omnivision/OmniVisionInfo" which contains messages about the soccer ball and all the robots' information such as position, velocity and etc. like the functions of an omnivision camera. Since there may be multiple robots, these topics or services names should be prefixed with the robot model names in order to distinguish between each other. For example, if your robot model's name is "nubot1", then the topic names are "/nubot1/nubotcontrol/velcmd" and "/nubot1/omnivision/OmniVisionInfo" and the service names would be changed to "/nubot1/BallHandle" and "/nubot1/Shoot" accordingly. You can customize this code for your robot based on these messages and services as a convenient interface. The types and definitions of the topics and servivces are listed in the following table:

Topic/Service Type Definition
/nubot1/nubotcontrol/velcmd nubot_common/VelCmd float32 Vx
float32 Vy
float32 w
/nubot1/omnivision/OmniVisionInfo ubot_common/OminiVisionInfo Header header
BallInfo ballinfo
ObstaclesInfo obstacleinfo
RobotInfo[] robotinfo
/nubot1/BallHandle nubot_common/BallHandle int64 enable
---
int64 BallIsHolding
/nubot1/Shoot nubot_common/Shoot int64 strength
int64 ShootPos
---
int64 ShootIsDone

For the definition of "/BallHandle" service, when "enable" equals to a non-zero number, a dribble request would be sent. If the robot meets the conditions to dribble the ball, the service response "BallIsHolding" is true.

For the definition of "/Shoot" service, when "ShootPos" equals to -1, this is a ground pass. In this case, "strength" is the inital speed you would like the soccer ball to have. When "ShootPos" equals to 1, this is a lob shot. In this case, "strength" is useless since the strength is calculated by the Gazebo plugin automatically and the soccer ball would follow a parabola path to enter the goal area. If the robot successfully kicks the ball out even if it failed to goal, the service response "ShootIsDone" is true.

For the definition of the "omnivision/OmniVisionInfo" topic, there are three new message types: "BallInfo", "ObstaclesInfo" and "RoboInfo". The field "robotinfo" is a vector. Before introducing the format of these new messages, three other message types "Point2d", "PPoint" and "Angle" are used in their definitions:

# Point2d.msg, reperesenting a 2-D point.
float32 x				# x component
float32 y				# y component
# PPoint.msg, representing a 2-D point in polar coordinates.
float32 angle				# angle against polar axis
float32 radius				# distance from the origin
# Angle.msg, representing the angle
float32 theta				# angle of rotation

# BallInfo.msg, representing the information about the ball
Header header                           # a ROS header message defined by ROS package std_msgs
int32     ballinfostate	                # the state of the ball information; 
Point2d   pos                           # ball position in global reference frame; the origin of the frame is the center of
                                        # the soccer field; x-axis pointed horizentally towards the opponet's center of the
                                        # goal area and y-axis vertiacal to the x-axis using the right-hand rule
PPoint    real_pos                      # ball relative position in robot body frame; the origin of the body frame is the
                                        # center of the robot base; x-axis along the kicking-mechanism and y-axis vertical
                                        # to the x-axis using the right-hand rule
Point2d   velocity                      # ball velocity in global reference frame
bool      pos_known                     # ball position is known(1) or not(0)
bool      velocity_known                # ball velocity is known(1) or not(0)
# ObstaclesInfo.msg, representing the information about obstacles
Header header                           # a ROS header message defined by ROS package std_msgs
Point2d[] pos                           # obstacle position in global reference frame
PPoint[] polar_pos                      # obstable position in the polar frame of which the origin is the center of the
                                        # robot and the polar axis is along the kicking mechanism
# RobotInfo.msg, representing teammates' information
Header header                           # a ROS header message defined by ROS package std_msgs
int32    AgentID                        # ID of the robot
int32    targetNum1                     # robot ID to be assigned target position 1
int32    targetNum2                     # robot ID to be assigned target position 2
int32    targetNum3                     # robot ID to be assigned target position 3
int32    targetNum4                     # robot ID to be assigned target position 4
int32    staticpassNum                  # in static pass, the passer's ID
int32    staticcatchNum                 # in static pass, the catcher's ID
Point2d  pos                            # robot position in global reference frame
Angle    heading                        # robot heading in global reference frame
float32  vrot                           # the rotational velcoity in global reference frame
Point2d  vtrans                         # the linear velocity in global reference frame
bool     iskick                         # robot kicks the ball(1) or not(0)
bool     isvalid                        # robot is valid(1) or not(0)
bool     isstuck                        # robot is stuck(1) or not(0)
bool     isdribble                      # robot dribbles the ball(1) or not(0)
char     current_role                   # the current role
float32  role_time                      # time that the robot keeps the role unchanged
Point2d  target	                        # target position

The units of these elements are cm, cm/s, rad and rad/s.

Configuration of computer A and computer B

config

The recommended way to run simulation is with two computers running nubot_ws and gazebo_visual seperately.

For example,computer A runs gazebo_visual to display the movement of robots. Computer B runs nubot_ws to calculate and send movement commands to robots. In addition, computer B should also run coach to send game command such as game start.

The communication between computer A and computer B is via ROS master. The following is the configuration steps:

  1. In computer A, add computer B's IP address in /etc/hosts; and in computer B, add computer A's IP address in /etc/hosts e.g. In computer A, $ sudo gedit /etc/hosts and add "Maggie 192.168.8.100" In computer B, $ sudo gedit /etc/hosts and add "Bart 192.168.8.101"
  2. In computer A, run gazebo_visual; In computer B, before you run nubot_ws, you should export ROS_MASTER_URI. e.g. In computer B, $ export ROS_MASTER_URI=http://Bart:11311
  3. In computer B, run coach and send game command

To run gazeo_visual

  1. ./configure
  2. $ source devel/setup.bash
  3. $ roslaunch nubot_gazebo game_ready.launch You can edit global_config to change simulation parameters such as the number of robots

To run nubot_ws

Note: Please remember to set the SIMULATION flag first! About how to set this flag, please refer to the documentation of nubot_ws.

$ ./src/cyan_spawn_model.sh
or
$ ./src/magenta_spawn_model.sh

To run coach4sim

  1. $ source devel/setup.bash
  2. $ roslaunch nubot_coach cyan_sim.launch
    or
    $ roslaunch nubot_coach magenta_sim.launch

Citation

If you use this simulation system, please cite our work.

@inproceedings{yao2015simulation,
  title={A simulation system based on ros and gazebo for robocup middle size league},
  author={Yao, Weijia and Dai, Wei and Xiao, Junhao and Lu, Huimin and Zheng, Zhiqiang},
  booktitle={Robotics and Biomimetics (ROBIO), 2015 IEEE International Conference on},
  pages={54--59},
  year={2015},
  organization={IEEE}
}

@incollection{xiao2017building,
  title={Building software system and simulation environment for robocup Msl soccer robots based on ros and gazebo},
  author={Xiao, Junhao and Xiong, Dan and Yao, Weijia and Yu, Qinghua and Lu, Huimin and Zheng, Zhiqiang},
  booktitle={Robot operating system (ROS)},
  pages={597--631},
  year={2017},
  publisher={Springer}
}

About

No description, website, or topics provided.

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published