How to use actionlib simple action server in gazebo model plugin

asked 2020-06-05 11:33:49 -0500

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)
{

}
edit retag flag offensive close merge delete

Comments

Hi @prithupareek,

I think in my humble opinion that you should change your current paradigm since I think it is not coherent with the "modular" approach of ROS.

I would generate different nodes for what you are trying to do to keep it simple and less error prone. You can implement two nodes for the client-server action lib communication and a last node implementing a Model plugin that reads from the command topic from the actionlib server. With that subscriber you will able to extract command data from ROS actionlib and apply the necessary commands on your gazebo model. I have never try to include a actiolib server into a Gazebo plugin but this is because, for me, there is no point in running both at the same scope since actionlib was not designed with that in mind; one of the most important reason may be that the actionlib server ...(more)

Weasfas gravatar image Weasfas  ( 2020-06-07 08:46:38 -0500 )edit