ROS(indigo) 安装和使用更新版本的Gazebo----3,4,5,6,7 附：中国机器人大赛中型组仿真比赛说明

~$ sudo apt-get install ros-indigo-desktop-full

sudo apt-get remove ros-indigo-desktop-full 

sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'

~$ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
--2016-11-24 12:39:26--  http://packages.osrfoundation.org/gazebo.key
Resolving packages.osrfoundation.org (packages.osrfoundation.org)... 54.193.183.180
Connecting to packages.osrfoundation.org (packages.osrfoundation.org)|54.193.183.180|:80... [sudo] password for relaybot: connected.
HTTP request sent, awaiting response... 200 OK
Length: 1772 (1.7K) [application/pgp-keys]
Saving to: ‘STDOUT’

100%[======================================>] 1,772       --.-K/s   in 0s      

2016-11-24 12:39:26 (7.08 MB/s) - written to stdout [1772/1772]

OK

sudo apt-get update

 sudo apt-get install gazebo
gazebo2                         gazebo5-plugin-base-prerelease
gazebo2-dbg                     gazebo5-prerelease
gazebo3                         gazebo6
gazebo3-common                  gazebo6-common
gazebo3-dbg                     gazebo6-common-prerelease
gazebo3-doc                     gazebo6-dbg
gazebo3-plugin-base             gazebo6-dbg-prerelease
gazebo4                         gazebo6-doc
gazebo4-common                  gazebo6-doc-prerelease
gazebo4-common-prerelease       gazebo6-plugin-base
gazebo4-dbg                     gazebo6-plugin-base-prerelease
gazebo4-dbg-prerelease          gazebo6-prerelease
gazebo4-doc                     gazebo6-robocup3ds
gazebo4-doc-prerelease          gazebo6-robocup3ds-common
gazebo4-plugin-base             gazebo7
gazebo4-plugin-base-prerelease  gazebo7-common
gazebo4-prerelease              gazebo7-dbg
gazebo5                         gazebo7-doc
gazebo5-build-deps              gazebo7-haptix
gazebo5-common                  gazebo7-haptix-common
gazebo5-common-prerelease       gazebo7-haptix-dbg
gazebo5-dbg                     gazebo7-haptix-doc
gazebo5-dbg-prerelease          gazebo7-haptix-plugin-base
gazebo5-doc                     gazebo7-plugin-base
gazebo5-doc-prerelease          gazebo7-robocup3ds
gazebo5-plugin-base             gazebo7-robocup3ds-common

~$ sudo apt-get install libgazebo
libgazebo3                 libgazebo6-dev
libgazebo4                 libgazebo6-dev-prerelease
libgazebo4-dev             libgazebo6-prerelease
libgazebo4-dev-prerelease  libgazebo6-robocup3ds
libgazebo4-prerelease      libgazebo7
libgazebo5                 libgazebo7-dev
libgazebo5-dbg             libgazebo7-haptix
libgazebo5-dev             libgazebo7-haptix-dev
libgazebo5-dev-prerelease  libgazebo7-robocup3ds
libgazebo5-prerelease      libgazebo-dev
libgazebo6  

sudo apt-get install gazebo7

gazebo

sudo apt-get install ros-indigo-desktop

:~$ sudo apt-get install ros-indigo-gazebo7-
ros-indigo-gazebo7-msgs         ros-indigo-gazebo7-ros-control
ros-indigo-gazebo7-plugins      ros-indigo-gazebo7-ros-pkgs
ros-indigo-gazebo7-ros

~$ roslaunch robot1_gazebo gazebo_wg.launch 
... logging to /home/relaybot/.ros/log/247f85e8-b203-11e6-85e0-0811968dc4b0/roslaunch-relay-ThinkPad-T420s-4397.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://relay-ThinkPad-T420s:59553/

SUMMARY
========

PARAMETERS
 * /robot_description: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    robot_state_publisher (robot_state_publisher/state_publisher)
    urdf_spawner (gazebo_ros/spawn_model)

auto-starting new master
process[master]: started with pid [4412]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 247f85e8-b203-11e6-85e0-0811968dc4b0
process[rosout-1]: started with pid [4425]
started core service [/rosout]
process[gazebo-2]: started with pid [4432]
process[gazebo_gui-3]: started with pid [4441]
process[robot_state_publisher-4]: started with pid [4449]
process[urdf_spawner-5]: started with pid [4457]
spawn_model script started
[ INFO] [1479963726.738918650]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1479963726.740184907]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[INFO] [WallTime: 1479963726.793513] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1479963726.796361] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1479963727.401217] [0.000000] Calling service /gazebo/spawn_urdf_model
[ INFO] [1479963727.884869698, 0.001000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1479963727.891392402, 0.001000000]: Laser Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1479963727.891614468, 0.001000000]: Starting Laser Plugin (ns = /)!
[INFO] [WallTime: 1479963727.899568] [0.001000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1479963727.910095653, 0.001000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1479963727.915187439, 0.001000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1479963727.948279641, 0.001000000]: Loading gazebo_ros_control plugin
[ INFO] [1479963727.948487708, 0.001000000]: Starting gazebo_ros_control plugin in namespace: /robot
[ INFO] [1479963727.950724362, 0.001000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [/robot_description] on the ROS param server.
[urdf_spawner-5] process has finished cleanly
log file: /home/relaybot/.ros/log/247f85e8-b203-11e6-85e0-0811968dc4b0/urdf_spawner-5*.log
[ INFO] [1479963728.200219231, 0.001000000]: Loaded gazebo_ros_control.
[ INFO] [1479963728.205156602, 0.001000000]: Starting GazeboRosSkidSteerDrive Plugin (ns = /)!
[ INFO] [1479963728.242052698, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1479963728.346945001, 0.110000000]: Physics dynamic reconfigure ready

~$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently:	speed 0.5	turn 1 

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

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
                };

# Uses ISO units. For example, use 'm' for length.
general:
  dribble_distance_thres: 0.47           # theshold distance between nubot and football below which dribble ball
  dribble_angle_thres: 15.0             # kicking mechanism aligning with football; allowed maximum angle error in degrees
  noise_scale: 0.00                     # the scale of gaussian noise (m)
  noise_rate: 0.01                       # how frequent the noise is generated

cyan:
  prefix: "nubot"             # Nubot name prefix. 
  num: 3

magenta:
  prefix: "rival"            # Rival name prefix. 
  num: 3

field:
  length: 18                     # Use integer!   used in spawn_model_script
  width: 12                      # Use integer!  used in spawn_model_script

football:
  name: "football"                   # football model name
  chassis_link: "football::chassis"     # football body link name  

# 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

/** 主要的控制框架位于这里*/
    void
    loopControl(const ros::TimerEvent& event)
    {
        match_mode_ = world_model_info_.CoachInfo_.MatchMode;               //! 当前比赛模式
        pre_match_mode_ = world_model_info_.CoachInfo_.MatchType;           //! 上一个比赛模式
        robot_pos_  = world_model_info_.RobotInfo_[world_model_info_.AgentID_-1].getLocation();
        robot_ori_  = world_model_info_.RobotInfo_[world_model_info_.AgentID_-1].getHead();
        ball_pos_   = world_model_info_.BallInfo_[world_model_info_.AgentID_-1].getGlobalLocation();
        ball_vel_   = world_model_info_.BallInfo_[world_model_info_.AgentID_-1].getVelocity();
        nubot_common::VelCmd        vel;
        nubot_common::Shoot         shoot;
        nubot_common::BallHandle    dribble;

        if(match_mode_ == STOPROBOT )
        {
            vel.Vx = 0;
            vel.Vy = 0;
            vel.w  = 0;
            motor_cmd_pub_.publish(vel);
            shoot.request.strength = 0;
            shoot_client_.call(shoot);
            dribble.request.enable = 0;
            ballhandle_client_.call(dribble);
        }
        /** 机器人在开始之前的跑位. 开始静态传接球的目标点计算*/
        else if(match_mode_ > STOPROBOT && match_mode_ <= DROPBALL)
            positioning();
        else if(match_mode_==PARKINGROBOT)
            parking();
        else// 机器人正式比赛了，进入start之后的机器人状态
        {
            normalGame();
        } // start部分结束

        pubStrategyInfo();  // 发送策略消息让其他机器人看到，这一部分一般用于多机器人之间的协同
    }

    void positioning()
    {
        DPoint br = ball_pos_ - robot_pos_;
        switch(world_model_info_.AgentID_)  // 十分简单的实现，固定的站位，建议动态调整站位，写入staticpass.cpp中
        {                                   // 站位还需要考虑是否犯规，但是现在这个程序没有考虑。
            case 1:
                if(move2target(DPoint(-850.0, 0.0), robot_pos_))
                    move2ori(br.angle().radian_, robot_ori_.radian_);
            break;
            case 2:
                if(move2target(DPoint(-150.0, 100.0), robot_pos_))
                    move2ori(br.angle().radian_, robot_ori_.radian_);
            break;
            case 3:
                if(move2target(DPoint(-150.0, -100.0), robot_pos_))
                    move2ori(br.angle().radian_, robot_ori_.radian_);
            break;
            case 4:
                if(move2target(DPoint(-450.0, 200.0), robot_pos_))
                    move2ori(br.angle().radian_, robot_ori_.radian_);
            break;
            case 5:
                if(move2target(DPoint(-450.0, -200.0), robot_pos_))
                    move2ori(br.angle().radian_, robot_ori_.radian_);
            break;
        }
    }

    void parking()
    {
        static double parking_y=-580.0;
        cout<<"PARKINGROBOT"<<endl;

        DPoint parking_target;
        float tar_ori = SINGLEPI_CONSTANT/2.0;
        parking_target.x_= -120.0 * world_model_info_.AgentID_;
        if(world_model_info_.AgentID_ == 1)
            parking_target.x_ = -700;//守门员站在离球门最近的地方
        parking_target.y_ = parking_y;

        if(move2target(parking_target, robot_pos_))    //停到目标点10cm附近就不用动了，只需调整朝向
            move2ori(tar_ori, robot_ori_.radian_);
    }

    void normalGame()
    {
        if(world_model_info_.AgentID_ != 1 && isNearestRobot())
        {
            nubot_common::BallHandle    dribble;
            DPoint br = ball_pos_ - robot_pos_;

            if(move2ori(br.angle().radian_, robot_ori_.radian_))        // 先往足球靠近
            {
                if(move2target(ball_pos_, robot_pos_, 50.0))
                {
                    dribble.request.enable = 1;
                    ballhandle_client_.call(dribble);
                    if(dribble.response.BallIsHolding != true)
                    {
                        if(move2target(ball_pos_, robot_pos_, 40.0))
                            move2ori(br.angle().radian_, robot_ori_.radian_);
                    }
                    else        // 带上球了
                    {
                        DPoint tmp(200.0,300.0);
                        DPoint dirc = DPoint(900.0 ,0.0) - tmp;         // 对准 (900.0 ,0.0)
                        if(move2target(tmp, robot_pos_) &&
                           move2ori(dirc.angle().radian_, robot_ori_.radian_, 5.0*DEG2RAD))  // 跑到位以及转到位
                        {
                            nubot_common::Shoot shoot;
                            shoot.request.ShootPos = FLY;
                            shoot.request.strength = 1.0;   // 在FLY模式下，strength不重要,只要非零就行
                            shoot_client_.call(shoot);
                        }
                    }
                }
            }
        }
    }

    bool isNearestRobot()         //找到距离足球最近的机器人
    {
        float distance_min = 2000.0;
        float distance = distance_min;
        int robot_id = -1;

        for(int i=1;i<OUR_TEAM;i++)     // 排除守门员
            if(world_model_info_.RobotInfo_[i].isValid())
            {
                distance = ball_pos_.distance(world_model_info_.RobotInfo_[i].getLocation());
                if(distance < distance_min)
                {
                    distance_min=distance;
                    robot_id = i;
                }
            }
        if(robot_id+1 == world_model_info_.AgentID_)
            return true;
        else
            return false;
    }

    bool move2target(DPoint target, DPoint pos, double distance_thres=10.0)     // 一个十分简单的实现，可以用PID
    {
        static nubot_common::VelCmd        vel;
        DPoint tmp = target - pos;
        float tar_theta = tmp.angle().radian_;

        if(tmp.norm() > distance_thres)
        {
            vel.Vx = tmp.norm()*0.7*cos(tar_theta - robot_ori_.radian_);    // 注意将全局坐标系下的期望速度转换为在机器人体坐标系下
            vel.Vy = tmp.norm()*0.7*sin(tar_theta - robot_ori_.radian_);
            vel.w = 0;
            motor_cmd_pub_.publish(vel);
            return false;
        }
        else
        {
            vel.Vx = 0.0;
            vel.Vy = 0.0;
            vel.w = 0;
            motor_cmd_pub_.publish(vel);
            return true;
        }
    }

    bool move2ori(double target, double angle, double angle_thres = 8.0*DEG2RAD)  // 一个十分简单的实现，可以用PID
    {
        static nubot_common::VelCmd        vel;
        double tmp = target - angle;
        if(fabs(tmp) > angle_thres)        // 容许误差为5度
        {
            vel.Vx = 0;
            vel.Vy = 0;
            vel.w = tmp;
            motor_cmd_pub_.publish(vel);
            return false;
        }
        else
        {
            vel.Vx = 0.0;
            vel.Vy = 0.0;
            vel.w = 0;
            motor_cmd_pub_.publish(vel);
            return true;
        }
    }

    void pubStrategyInfo()
    {
        nubot_common::StrategyInfo strategy_info;       // 这个消息的定义可以根据个人需要进行修改
        strategy_info.header.stamp = ros::Time::now();
        strategy_info.AgentID     = world_model_info_.AgentID_;

        strategy_info_pub_.publish(strategy_info);
    }

    void pubStrategyInfo()
    {
        nubot_common::StrategyInfo strategy_info;       // 这个消息的定义可以根据个人需要进行修改
        strategy_info.header.stamp = ros::Time::now();
        strategy_info.AgentID     = world_model_info_.AgentID_;

        strategy_info_pub_.publish(strategy_info);
    }

/**  仿真程序更新所有的机器人的策略信息，在实际比赛中通过RTDB进行传输，现在采用topic传输*/
void
nubot::World_Model::updateStrategyinfo(const nubot_common::simulation_strategy &_strategyinfo)
{
    int nums_robots = _strategyinfo.strategy_msgs.size();
    for(int i =0 ; i < nums_robots; i++)
    {
        nubot_common::StrategyInfo strategyinfo = _strategyinfo.strategy_msgs[i];
        int AgentId = strategyinfo.AgentID;
        if(AgentId < 1 || AgentId >OUR_TEAM)
            continue;
        PassCommands & pass_cmd_  = teammatesinfo_[AgentId-1].pass_cmds_;
        pass_cmd_.catchrobot_id   = strategyinfo.pass_cmd.catch_id;
        pass_cmd_.passrobot_id    = strategyinfo.pass_cmd.pass_id;
        pass_cmd_.is_dynamic_pass = strategyinfo.pass_cmd.is_dynamic_pass;
        pass_cmd_.is_static_pass  = strategyinfo.pass_cmd.is_static_pass;
        pass_cmd_.is_passout = strategyinfo.pass_cmd.is_passout;
        pass_cmd_.pass_pt    = DPoint(strategyinfo.pass_cmd.pass_pt.x,strategyinfo.pass_cmd.pass_pt.y);
        pass_cmd_.catch_pt   = DPoint(strategyinfo.pass_cmd.catch_pt.x,strategyinfo.pass_cmd.catch_pt.y);
        pass_cmd_.isvalid    = strategyinfo.pass_cmd.is_valid;
        teammatesinfo_[AgentId-1].robot_info_.setRolePreserveTime(strategyinfo.role_time);
        teammatesinfo_[AgentId-1].robot_info_.setCurrentRole(strategyinfo.role);
        teammatesinfo_[AgentId-1].robot_info_.setDribbleState(strategyinfo.is_dribble);
        teammatesinfo_[AgentId-1].robot_info_.setCurrentAction(strategyinfo.action);

        teammatesinfo_[AgentId-1].robot_info_.setTargetNum(1,strategyinfo.targetNum1);
        teammatesinfo_[AgentId-1].robot_info_.setTargetNum(2,strategyinfo.targetNum2);
        teammatesinfo_[AgentId-1].robot_info_.setTargetNum(3,strategyinfo.targetNum3);
        teammatesinfo_[AgentId-1].robot_info_.setTargetNum(4,strategyinfo.targetNum4);

        teammatesinfo_[AgentId-1].robot_info_.setcatchNum(strategyinfo.staticcatchNum);
        teammatesinfo_[AgentId-1].robot_info_.setpassNum(strategyinfo.staticpassNum);
    }
}