ros1 supervisor services

[Tifus15]

Hello!
I stepped on two problems this and last week.

First affter I start my simulation in webots with ros service supervisor/self_node constantly give me 0.
I used your code from complete_test.cpp in webots_ros package.

Secondly I thought that services in supervisor like /get_pose, /get_orientation, /get_position would give topics even as combination with service /supervisor/node/enable_pose_tracking. Somehow without topic which should be geometry_msgs Pose I don't see the point of this service.

I need Pose topic for my robot that I can evaluate it in rviz. Thanks for help!

Denis Andrić

[lukicdarkoo]

First affter I start my simulation in webots with ros service supervisor/self_node constantly give me 0.
I used your code from complete_test.cpp in webots_ros package.

Can you provide the code so we can try to reproduce it?

[Tifus15]

#include "ros/ros.h"
#include "webots_ros/set_int.h"
#include "webots_ros/node_get_pose.h"
#include "webots_ros/get_uint64.h"
#include "webots_ros/node_enable_pose_tracking.h"
#include "webots_ros/node_get_orientation.h"
#include "std_msgs/String.h"



static std::vector<std::string> controllerList;
static int controllerCount;
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}


void quit(int sig) {
  ROS_INFO("User stopped the node.");
  ros::shutdown();
  exit(0);
}




int main(int argc, char **argv) {
    ros::init(argc, argv, "pose_publisher", ros::init_options::AnonymousName);
    ros::NodeHandle n;
    std::string controllerName;

    int time_step = 32;



    // subscribe to the topic model_name to get the list of availables controllers
    ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
        ros::Duration(0.5).sleep();
    }
    ros::spinOnce();
  
    // if there is more than one controller available, let the user choose
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        std::cout << "Choose the # of the controller you want to use:\n";
        std::cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount){
            controllerName = controllerList[wantedController - 1];
            ROS_INFO("CONTROLLER NAME: %s\n",controllerName);
        }
        else {
            ROS_ERROR("Invalid number for controller choice.");
            return 1;
        }
    }
    // leave topic once it's not necessary anymore
    nameSub.shutdown();
    ros::ServiceClient time_step_client;
    time_step_client = n.serviceClient<webots_ros::set_int>(controllerName + "/robot/time_step");
    webots_ros::set_int time_step_srv;
    time_step_srv.request.value = time_step;

    if (time_step_client.call(time_step_srv) && time_step_srv.response.success)
    ROS_INFO("time_step service works.");
        else ROS_ERROR("Failed to call service time_step to update robot's time step.");
    //get self should give Robot node
    
    ros::ServiceClient supervisor_get_self_client;
    webots_ros::get_uint64 supervisor_get_self_srv;
    supervisor_get_self_client = n.serviceClient<webots_ros::get_uint64>(controllerName + "/supervisor/get_self");

    supervisor_get_self_client.call(supervisor_get_self_srv);
    ROS_INFO("Got self node: %lu.", supervisor_get_self_srv.response.value);
    uint64_t self_node = supervisor_get_self_srv.response.value;

    supervisor_get_self_client.shutdown();

    ros::ServiceClient supervisor_node_get_orientation_client;
    webots_ros::node_get_orientation supervisor_node_get_orientation_srv;
    supervisor_node_get_orientation_client =
    n.serviceClient<webots_ros::node_get_orientation>(controllerName+ "/supervisor/node/get_orientation");

    supervisor_node_get_orientation_srv.request.node = self_node;
    supervisor_node_get_orientation_client.call(supervisor_node_get_orientation_srv);
    ROS_INFO(
    "From_def orientation quaternion is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_orientation_srv.response.orientation.w,
    supervisor_node_get_orientation_srv.response.orientation.x, supervisor_node_get_orientation_srv.response.orientation.y,
    supervisor_node_get_orientation_srv.response.orientation.z);

    supervisor_node_get_orientation_client.shutdown();
    time_step_client.call(time_step_srv);



    

    //get pose for broadcasting
    ros::ServiceClient supervisor_get_pose_client;
    webots_ros::node_get_pose supervisor_get_pose_srv;
    supervisor_get_pose_client = n.serviceClient<webots_ros::node_get_pose>(controllerName + "/supervisor/node/get_pose");

  

    supervisor_get_pose_srv.request.from_node = self_node;
    supervisor_get_pose_srv.request.node = self_node;
    supervisor_get_pose_client.call(supervisor_get_pose_srv);
    ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", supervisor_get_pose_srv.response.pose.rotation.w,
           supervisor_get_pose_srv.response.pose.rotation.x, supervisor_get_pose_srv.response.pose.rotation.y,
           supervisor_get_pose_srv.response.pose.rotation.z);
    ROS_INFO("From_def get_pose translation is:\nx=%f y=%f z=%f.", supervisor_get_pose_srv.response.pose.translation.x,
           supervisor_get_pose_srv.response.pose.translation.y, supervisor_get_pose_srv.response.pose.translation.z);

   // supervisor_get_pose_client.shutdown();
    time_step_client.call(time_step_srv);


    // supervisor_node_enable_pose_tracking
  ros::ServiceClient supervisor_node_enable_pose_tracking_client;
  webots_ros::node_enable_pose_tracking supervisor_node_enable_pose_tracking_srv;
  supervisor_node_enable_pose_tracking_client = n.serviceClient<webots_ros::node_enable_pose_tracking>(controllerName + "/supervisor/node/enable_pose_tracking");

  supervisor_node_enable_pose_tracking_srv.request.from_node = self_node;
  supervisor_node_enable_pose_tracking_srv.request.node = self_node;
  supervisor_node_enable_pose_tracking_srv.request.sampling_period = time_step;
  supervisor_node_enable_pose_tracking_client.call(supervisor_node_enable_pose_tracking_srv);
  supervisor_node_enable_pose_tracking_client.shutdown();
  time_step_client.call(time_step_srv);

  return 0;
}