ROS Action Servers in V-Rep plugins

Typically: "How do I... ", "How can I... " questions
Post Reply
francofusco
Posts: 9
Joined: 22 Apr 2017, 09:20

ROS Action Servers in V-Rep plugins

Post by francofusco » 19 May 2017, 14:03

Hello!

I was wandering how is it possible to "wrap" an Action Server within a V-Rep plugin.

As a quick trial, I took the AveragingAction class (http://wiki.ros.org/actionlib_tutorials ... kMethod%29) and modified as follows:

Code: Select all

class MyAveragingAction
{
public:
    
  MyAveragingAction(std::string name) : action_name_(name) {}
  ~MyAveragingAction(void){}
  
  void init() {
    nh_ = new ros::NodeHandle("/my_namespace");
    
    //register the goal and feeback callbacks
    as_ = new actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction>(*nh_, action_name_, false);
    as_->registerGoalCallback(boost::bind(&MyAveragingAction::goalCB, this));
    as_->registerPreemptCallback(boost::bind(&MyAveragingAction::preemptCB, this));

    // subscribe to the data topic of interest
    sub_ = nh_->subscribe("/random_number", 1, &MyAveragingAction::analysisCB, this);
    as_->start();
    ROS_INFO("Server started");
  }
  
  void goalCB() { ... } // unchanged
  void preemptCB() { ... } // unchanged
  void analysisCB(const std_msgs::Float32::ConstPtr& msg) { ... }  // unchanged
  
protected:
    
  ros::NodeHandle *nh_; // Now it is a pointer
  actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> *as_; // now it is a pointer
  // remaining fields remained unchanged
}
(The "big" difference with the original is that the SimpleActionServer is created within the init() method...)

I then added to vrep_plugin_skeleton.cpp the declaration of an instance of MyAveragingAction, and within the method v_repStart() I simply call myActionServer::init().

I was able to compile and load the plugin, and in addition the server is... serving! However, does anyone know if this is a wrong/discouraged way of working? If so, could you give me some references?

Thanks in advance!

fferri
Posts: 92
Joined: 09 Sep 2013, 19:28

Re: ROS Action Servers in V-Rep plugins

Post by fferri » 19 May 2017, 23:01

Hi Franco,

please post your code (vrep_plugin_skeleton.cpp) so we can review it.

There's nothing peculiar in having a ROS node inside a plugin. Remember to call spinOnce where appropriate (see the existing ROS or RosInterface plugins)

francofusco
Posts: 9
Joined: 22 Apr 2017, 09:20

Re: ROS Action Servers in V-Rep plugins

Post by francofusco » 22 May 2017, 09:32

Hello Federico,

Here is the code of "vrep_plugin_skeleton.cpp" (only the v_repStart method, since I kept the rest unchanged!):

Code: Select all

// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
    // Dynamically load and bind V-REP functions:
    // ******************************************
    // 1. Figure out this plugin's directory:
    char curDirAndFile[1024];
    getcwd(curDirAndFile, sizeof(curDirAndFile));

    std::string currentDirAndPath(curDirAndFile);
    // 2. Append the V-REP library's name:
    std::string temp(currentDirAndPath);
    #ifdef _WIN32
        temp+="\\v_rep.dll";
    #elif defined (__linux)
        temp+="/libv_rep.so";
    #elif defined (__APPLE__)
        temp+="/libv_rep.dylib";
    #endif

    // 3. Load the V-REP library:
    vrepLib=loadVrepLibrary(temp.c_str());
    if (vrepLib==NULL)
    {
        std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'RosAS' plugin.\n";
        return(0); // Means error, V-REP will unload this plugin
    }
    if (getVrepProcAddresses(vrepLib)==0)
    {
        std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'RosAS' plugin.\n";
        unloadVrepLibrary(vrepLib);
        return(0); // Means error, V-REP will unload this plugin
    }
    // ******************************************

    // Check the version of V-REP:
    // ******************************************
    int vrepVer;
    simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
    if (vrepVer<30102) // if V-REP version is smaller than 3.01.02
    {
        std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'RosAS' plugin.\n";
        unloadVrepLibrary(vrepLib);
        return(0); // Means error, V-REP will unload this plugin
    }
    // ******************************************


    // Initialize the ROS part:
    if(!ROS_server::initialize())
    {
        std::cout << "ROS master is not running. Cannot start 'RosAS' plugin.\n";
        return (0); //If the master is not running then the plugin is not loaded.
    }

    // Initialize the action server
    mysrv.init();
    
    return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
As I explained, I am calling mysrv.init() after having checked that ROS core is properly running using ROS_server::initialize() from the original ROS plugin (it's a lazy solution, I am planning to do something better in the future!).

This was obvioulsy a very simple example. As you suggest, I will try to have a deeper look at the available ROS plugins, so as to understand how to create something more functional.


In addition, I post here the link to another example I created: https://drive.google.com/file/d/0B8skE9 ... sp=sharing (I discourage to use it as a reference for new projects :P I am a novice and I don't want your PC to explode due to my inexperience!)

In this example, I use a very basic action called "ReachTarget". It asks "something" to reach a target value (whatever this means), allowing to specify a "tolerance". I use this action to specify a joint position, asking the actuator to reach it. I added some simple Lua functions, that allows the serving object to interface with the action server. The idea behind my plugin is not very complex: I want actions to be performed only if an object explicitly notifies the server that he's going to execute the job. After this "acknowledgement", the server will accept new goals (before, he had to reject them somehow). The object has to poll the server for new goal values (I don't know yet how to implement a callback-based mechanism, I will figure it out in the future). It will then execute the action; while doing so, it can transfer some data to the server in order to publish feedback messages. Finally, the object will notify the server that the action is completed (succesfully, if possible). At simulation end, I de-associate the object from the server, so that new goals won't be accepted until a new simulation (to be honest, this is not 100% true, since my implementation is a little weak right now).

In the included scene, I use the server to lift a platform. The "quick 'n' dirty" code inside the child script is associates the joint to the server, and then polls for new goals. As soon as a target is received, the target position of the joint is updated. After 2.5 seconds the actuator is fixed to the current achieved position, which may not correspond to the target (due to the presence of an obstructing object). The "distance" to the target is sent to the server. Depending on how far from the target we are, it will decide wether to set the state to "success" or to "abort". For those wishing to test the code, a simple client can be launched to actuate the joint:

Code: Select all

rosrun vrep_plugin_ros_action_server client_test [target_value]
I wish to improve the solution, by implementing a custom ActionServer interface (e.g., to "truly" reject goals when we are not in simulation) and by creating more reliable functions to interface with the server from child scripts. If anyone wants to have a look at the example and give me suggestions of any kind, I'd love it :)

fferri
Posts: 92
Joined: 09 Sep 2013, 19:28

Re: ROS Action Servers in V-Rep plugins

Post by fferri » 24 May 2017, 09:22

francofusco wrote: This was obvioulsy a very simple example. As you suggest, I will try to have a deeper look at the available ROS plugins, so as to understand how to create something more functional.


In addition, I post here the link to another example I created: https://drive.google.com/file/d/0B8skE9 ... sp=sharing (I discourage to use it as a reference for new projects :P I am a novice and I don't want your PC to explode due to my inexperience!)

In this example, I use a very basic action called "ReachTarget". It asks "something" to reach a target value (whatever this means), allowing to specify a "tolerance". I use this action to specify a joint position, asking the actuator to reach it. I added some simple Lua functions, that allows the serving object to interface with the action server. The idea behind my plugin is not very complex: I want actions to be performed only if an object explicitly notifies the server that he's going to execute the job. After this "acknowledgement", the server will accept new goals (before, he had to reject them somehow).
You are using the SimpleActionServer class. When you call acceptNewGoal () it will accept a new goal when one is available, and set it active, while any previously goal will be preempted.

If you want to be able to handle goals concurrently you have to use the ActionServer class.
francofusco wrote:The object has to poll the server for new goal values (I don't know yet how to implement a callback-based mechanism, I will figure it out in the future).
Indeed, it would be better if you wrap the (Simple)ActionServer's callbacks to Lua callbacks. You can call lua functions from C++ with simCallScriptFunctionEx. You can write any type of Lua value (numbers, strings, tables) to the function with the stack API
francofusco wrote:It will then execute the action; while doing so, it can transfer some data to the server in order to publish feedback messages. Finally, the object will notify the server that the action is completed (succesfully, if possible). At simulation end, I de-associate the object from the server, so that new goals won't be accepted until a new simulation (to be honest, this is not 100% true, since my implementation is a little weak right now).

In the included scene, I use the server to lift a platform. The "quick 'n' dirty" code inside the child script is associates the joint to the server, and then polls for new goals. As soon as a target is received, the target position of the joint is updated. After 2.5 seconds the actuator is fixed to the current achieved position, which may not correspond to the target (due to the presence of an obstructing object). The "distance" to the target is sent to the server. Depending on how far from the target we are, it will decide wether to set the state to "success" or to "abort". For those wishing to test the code, a simple client can be launched to actuate the joint:

Code: Select all

rosrun vrep_plugin_ros_action_server client_test [target_value]
I wish to improve the solution, by implementing a custom ActionServer interface (e.g., to "truly" reject goals when we are not in simulation) and by creating more reliable functions to interface with the server from child scripts. If anyone wants to have a look at the example and give me suggestions of any kind, I'd love it :)
As you can see, the ROS plugin already calls spinOnce when in simulation and also when not in simulation:

Code: Select all

void ROS_server::instancePass()
{ // When simulation is not running, we "spinOnce" here:
    int simState=simGetSimulationState();
    if ((simState&sim_simulation_advancing)==0)
        spinOnce();
}

void ROS_server::mainScriptAboutToBeCalled()
{ // When simulation is running, we "spinOnce" here:
    spinOnce();
}
You can detect when simulation is running in a similar fashion by calling simGetSimulationState() elsewhere.

francofusco
Posts: 9
Joined: 22 Apr 2017, 09:20

Re: ROS Action Servers in V-Rep plugins

Post by francofusco » 24 May 2017, 11:49

Hi Federico,

Thanks for your feedback, you gave me many useful suggestions. I will try to improve my code, based on what you suggested me.

Have a nice day!

Post Reply