public: void Load()

in simulation_ws/src/robot_fleet/src/move_object.cc [47:89]

• 29 lines of code
• 3 McCabe index (conditional complexity)

        public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
        {
            this->model = _parent;
            this->model_name = this->model->GetName();
#if GAZEBO_MAJOR_VERSION < 8
            this->current_pose = this->model->GetWorldPose();
#else
            this->current_pose = this->model->WorldPose();
#endif

            // Listen to the update event. This event is broadcast every simulation iteration.
            this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&MoveObject::OnUpdate, this));
            this->old_secs =ros::Time::now().toSec();

            // Initialize ros, if it has not 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;
            }

            // Create our ROS node. This acts in a similar manner to the Gazebo node
            this->rosNode.reset(new ros::NodeHandle(this->model->GetName()));

            this->rosNode->getParam(std::string("/use_custom_move_object_gazebo_plugin"), this->use_custom_move_object_gazebo_plugin);
            ROS_INFO_STREAM("use_move is set to: " << this->use_custom_move_object_gazebo_plugin);


            if (this->use_custom_move_object_gazebo_plugin)
            {
                // Store the pointer to the model

                // Subscribe to topic here. Note that if you pass model name, it adds namespace to the topic
                ros::SubscribeOptions sub_options = ros::SubscribeOptions::create<geometry_msgs::Pose>("topic_to_move", 1,
                                              boost::bind(&MoveObject::OnPoseCallback, this, _1),
                                              ros::VoidPtr(), &this->rosQueue);
                this->rosSubscription = this->rosNode->subscribe(sub_options);

                // Spin up the queue helper thread.
                this->rosQueueThread = std::thread(std::bind(&MoveObject::QueueThread, this));
            }
        }