#19为世界环境添加功能

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

一、写一个plugin

1.首先写一个开关程序：

#include <functional>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>

using namespace gazebo;

class WorldPluginTutorial : public WorldPlugin
{
    public:
        
        static bool getStatus()
        {
          return WorldPluginTutorial::status;
        }
        
        static void setSatus(bool state)
        {
          WorldPluginTutorial::status = state;
        }
        
        void toggleLight()
        {
          if (WorldPluginTutorial::getStatus())
          {
            this->world->InsertModelFile("model://sun");    //如果收到了开灯的信息，就给一个太阳。（表示开灯了）
            std::cout << "turned ON" << std::endl;
          }
          else
          {
            this->world->RemoveModel("sun");                //移除太阳
            std::cout << "turned OFF" << std::endl;
          }
        }
        
        WorldPluginTutorial(): WorldPlugin() //constructor
        {
          this->prev = 0;
        }
    
        void Load(physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/)
        {
          
          // Make sure the ROS node for Gazebo has already been initialized                                                                                    
          if (!ros::isInitialized())
          {
            ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
              << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
            return;
          }
          
          ROS_INFO("*** Switch Plugin loaded! ***");
      
          this->_state_pub = _nh.advertise<std_msgs::String>("/light_state", 5, true);
          this->_state_sub = _nh.subscribe("/power_grid", 100, powerCallback);
          
          this->world = _world;
          //this event is fired on every sim iteration
          this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&WorldPluginTutorial::OnUpdate, this));
          
        }
        
        void OnUpdate()   //用于控制灯的开关
        {
          
          
          if(ros::ok()) //the main loop
          {
            _ss.str(std::string()); //clear the sstream buffer
            
            if(WorldPluginTutorial::getStatus() && prev == 1)
            {
              _ss << "on";
              prev = 0;
              
              toggleLight();
              
              _light_msg.data = _ss.str();
              _state_pub.publish(_light_msg); //publish the current state of the light
              
            }
            
            if(!WorldPluginTutorial::getStatus() && prev == 0)
            {
              _ss << "off";
              prev = 1;
              
              toggleLight();
              
              _light_msg.data = _ss.str();
              _state_pub.publish(_light_msg); //publish the current state of the light
            }
            
          }
        }
        
        static void powerCallback(const std_msgs::String::ConstPtr& msg)  //接受消息，传给msg
        { 
          ROS_INFO("Received [%s]", msg->data.c_str()); 
          
          if(strcmp(msg->data.c_str(), "1") == 0)        //如果接受到的消息是1  
          {
            WorldPluginTutorial::setSatus(true);
          }
          else if(strcmp(msg->data.c_str(), "0") == 0)  //如果接收到的消息是0
          {
            WorldPluginTutorial::setSatus(false);
            
          }
          
        }
        
        
    private:
        ros::NodeHandle _nh;
        
        ros::Publisher _state_pub;
        std_msgs::String _light_msg;
        std::stringstream _ss;
        int prev;
        
        ros::Subscriber _state_sub;
        
        //pointer to world
        physics::WorldPtr world;
        // Pointer to the update event connection
        event::ConnectionPtr updateConnection;
        
        //static variables
        //for tracking the light toggles
        static bool status;
        
        

};
//initialize static variables
bool WorldPluginTutorial::status = true;

GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)

2.写cmake:

cmake_minimum_required(VERSION 2.8.3)
project(gazebo_world_plugin)

set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS 
  roscpp 
  gazebo_ros 
)

# Depend on system install of Gazebo
find_package(gazebo REQUIRED)

link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})

add_library(switch_node src/switch.cpp)
target_link_libraries(switch_node ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

catkin_package(
  DEPENDS 
    roscpp 
    gazebo_ros 
)

这是文件目录：

3.catkin_make 进行编译，得到build,devel

4.添加gazebo_plugin

export GAZEBO_PLUGIN_PATH="${GAZEBO_PLUGIN_PATH}:/home/rz/test/simulation_ws/devel/lib/"

gazebo_plugin：代表仿真环境中实现的各种功能，开个灯之类的，需要在编译之后导入，通常在/devel/lib内
gazebo_models:可以自建，可以从网上下载，代表的是物体的外观属性，可以直接导入

二、配置环境

1.将插件导入world中

如图，在world文件的最后将libswitch_node.so，插入其中（1.这玩意相当于一种动态环境，并且可以调控2.已经在gazebo中插入了plugin的路径）

这里复习以下工作区的分组：

two_wheels_description 是指的机器人
twi_wheels_gazebo指的是仿真环境
每一个目录下的cmake和package文件只能管着当前目录

2.进行仿真

roslaunch two_wheels_description room.launch

结果gazebo无法打开，很明显，忘记给models分配路径了！
这里分配路径：

然后就打开环境了！
roslaunch two_wheels_description room.launch

rostopic list 看一下主题：

light_state和power_grid就是创建出来的plugins
light_state是获得当前灯光的状态
power_grid是发送命令，控制灯
这都是对应的！

接收灯的状态：
rostopic echo /light_state
控制灯的状态：
rostopic pub /power_grid std_msgs/String “data: ‘0’”

本来还想使用key_teleop.py

键盘控制一下小车的运行和灯的开关（通过发送主题），谁知道这个文件是用python2写的，而noetic似乎不支持python2，故不能再演示了。