ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Problem with nodehandle before ros::init

asked 2013-11-13 00:06:24 -0500

mortenpj gravatar image

Hi

I have inserted a shortened version of my files below, but first i'll try to explain my problem. My problem is that I want to define a MoveTo() function within skills.cpp, which uses actionlib client acUR5. I can't define the MoveTo() function without defining acUr5 before the definition of the MoveTo() function in skills.cpp. The acUr5 needs ros::init(), which first have been defined in the main() function. Thus then I run execute_actionlib_client.cpp. I get the error showed in the bottom of this post.

execute_actionlib_client.cpp:

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <rq3_proxy/rq3Action.h>
#include <ur5_proxy/ur5Action.h>
#include "skills/skills.hpp"

int main (int argc, char **argv){

ros::init(argc, argv, "skills_node");

actionlib::SimpleActionClient<rq3_proxy::rq3Action> acRq3("rq3proxynode", true);
actionlib::SimpleActionClient<ur5_proxy::ur5Action> acUr5("ur5Node", true);

rq3_proxy::rq3Goal goalRq3;
ur5_proxy::ur5Goal goalUr5;

MoveTo();

Skills.cpp:

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <rq3_proxy/rq3Action.h>
#include <ur5_proxy/ur5Action.h>

#include "skills.hpp"

actionlib::SimpleActionClient<rq3_proxy::rq3Action> acRq3("rq3proxynode", true);
actionlib::SimpleActionClient<ur5_proxy::ur5Action> acUr5("ur5Node", true); // PROBLEM

rq3_proxy::rq3Goal goalRq3;
ur5_proxy::ur5Goal goalUr5;

void MoveTo(){
    //Viapoint, becore place
    acUr5.sendGoalAndWait(move("j", "car", 60,-508, 212, 1.7574, 0.7509, 1.714, 2, 0, 0, goalUr5),ros::Duration(30.0),ros::Duration(30.0));
    ROS_INFO("Viapoint 2 finished");
}

Problem when run:

private@pc:~/ros_ws/devel_ws$ rosrun skills execute_actionlib_client 
[FATAL] [1384343030.296120354]: You must call ros::init() before creating the first NodeHandle
[FATAL] [1384343030.296325757]: BREAKPOINT HIT
    file = /tmp/buildd/ros-groovy-roscpp-1.9.50-0precise-20131015-2117/src/libros/node_handle.cpp
    line=151

Trace/breakpoint trap (core dumped)

I hope you understand the problem and that you can help

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2013-11-13 00:47:09 -0500

mirzashah gravatar image

updated 2013-11-13 00:54:55 -0500

The two ActionClients in skills.cpp are attempting to create NodeHandle objects within their internal implementation: https://github.com/ros/actionlib/blob/hydro-devel/include/actionlib/client/action_client.h#L184

In the ROS C++ API (roscpp), you must always call ros::init before creating a NodeHandle.

As those are global variables, they are constructed before main() is even run. You must delay the construction of these objects until after main() has executed. The easiest way is to make the global variables pointers, create a function which sets those global variables, and then call that function after main() has started.

actionlib::SimpleActionClient<rq3_proxy::rq3Action>* acRq3;
actionlib::SimpleActionClient<ur5_proxy::ur5Action>* acUr5;

void initActionClients()
{
  acRq3 = new actionlib::SimpleActionClient("rq3proxynode", true);
  acUr5 = new actionlib::SimpleActionClient("acUr5", true);
}

int main()
{
  ros::init(argc, argv);
  initActionClients();
  //Be happy
}
edit flag offensive delete link more

Comments

Thanks for your answer! I made it work with a few corrections to your suggestion. I added the type to acRq3 and acUr5 in the function. Changed acUr5.sendGoalAndWait to acUr5->sendGoalAndWait. And I have passed an argument to the function MoveTo(acUr5).

mortenpj gravatar image mortenpj  ( 2013-11-13 02:11:53 -0500 )edit

ah yes i forgot the template type parameters...didn't actually test it. and yes the "->" are required for pointer types

mirzashah gravatar image mirzashah  ( 2013-11-13 03:55:37 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2013-11-13 00:06:24 -0500

Seen: 2,678 times

Last updated: Nov 13 '13