How to use actionlib simple action server in gazebo model plugin

edit

Hey all,

I hope that you are doing well. I am trying to use a simple action server in a gazebo model plugin in order to control a gazebo actor using ROS. For some reason, I am not able to see the various topics that should be created by the action server when running rostopic list.

I think that this is because I am not able to get ros::spin() to work properly. When I tried doing using that in the Load Function it gave me an error saying that I was trying to spin too many threads. I then tried to use an Async spinner, but that didnt' work either. I would really appreciate any help that you could provide on this matter, as I have looked everywhere and not found anything that works.

I have attached my code below.

/////////////////////////////////////////////////////////////////////
// Plugin to allow for ROS control of actors
// Prithu Pareek 2020
/////////////////////////////////////////////////////////////////////

#include <gazebo/physics/physics.hh>
#include <functional>
#include <iostream>
#include <ros/ros.h>

#include "ActorRosPlugin.hh"

#include <actionlib/server/simple_action_server.h>
#include <tbd_podi_gazebo/MoveActorAction.h>


using namespace gazebo;

// Register the plugin with gazebo
GZ_REGISTER_MODEL_PLUGIN(ActorRosPlugin);

/////////////////////////////////////////////////////////////////////
// CONSTRUCTOR
/////////////////////////////////////////////////////////////////////
ActorRosPlugin::ActorRosPlugin()
{
    // int argc = 0;
    // char** argv = NULL;

    // ros::init(argc, argv, "move_actor");


}

/////////////////////////////////////////////////////////////////////
// DESTRUCTOR
/////////////////////////////////////////////////////////////////////
ActorRosPlugin::~ActorRosPlugin()
{

}

/////////////////////////////////////////////////////////////////////
// LOAD THE ACTOR AND WORLD
/////////////////////////////////////////////////////////////////////
void ActorRosPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{



    printf("ActorRosPlugin Loaded\n");




    // ros::MultiThreadedSpinner spinner(0);
    // spinner.spin();

    ros::NodeHandle nh_;
    std::string name = "actor";

    actionlib::SimpleActionServer<tbd_podi_gazebo::MoveActorAction> as_(nh_, name, boost::bind(&ActorRosPlugin::executeCB, this, _1), false);

    as_.start();

    ros::AsyncSpinner spinner(4);

    spinner.start();



    // store a pointer to the sdf
    this->sdf = _sdf;

    // store a pointer to the actor
    this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);

    // store a pointer to the world
    this->world = this->actor->GetWorld();

    // Listen to the update event. This event is broadcast every simulation iteration
    this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
        std::bind(&ActorRosPlugin::OnUpdate, this, std::placeholders::_1)
    ));

    this->Reset();
}

/////////////////////////////////////////////////////////////////////
// REST THE WORLD
/////////////////////////////////////////////////////////////////////
void ActorRosPlugin::Reset()
{
    // init values for variables
    this->velocity = 0.8;
    this->lastUpdate = 0;

    // ros::spinOnce();

}

/////////////////////////////////////////////////////////////////////
// UPDATE THE WORLD
/////////////////////////////////////////////////////////////////////
void ActorRosPlugin::OnUpdate(const common::UpdateInfo &_info)
{
}

void ActorRosPlugin::executeCB(const tbd_podi_gazebo::MoveActorGoalConstPtr &goal)
{

}