How to call a ROS service with a launch file every time press the joystick to control UUV vehicle in Gazebo simulation?

Question

Im using Gazebo 9 and ROS melodic for some UUV Simulation. I created a ROS Service that pass the IMU sensors values from the Server to Client. Now I would like to use this service as a sensor feedback to the controller every time I control the UUV( means every time I press the joystick). Here are the Server and Client Node and the .srv files first

Server

#include "ros/ros.h"
#include <sensor_msgs/Imu.h>
#include "imu_service/ImuValue.h"

ros::ServiceServer service;
double current_x_orientation_s;
double get_imu_orientation_x;

bool get_val(imu_service::ImuValue::Request  &req, imu_service::ImuValue::Response &res)
{
    
    ROS_INFO("sending back response");    
    res.current_x_orientation_s = get_imu_orientation_x;
    //.. same for the other IMU values
        
}

void imuCallback(const  sensor_msgs::ImuConstPtr& msg)
{
  
     current_x_orientation_s= msg->orientation.x;
     get_imu_orientation_x=current_x_orientation_s;
     // ..same for other IMU values
           
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_status_server");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/thrbot/imu", 10, imuCallback);
  service = n.advertiseService("imu_status_server", get_val);
  ROS_INFO("Starting server...");
  ros::spin();
  return 0;
}

the Client is here

#include "ros/ros.h"
#include "ros_services/ImuValue.h"
#include <cstdlib>
#include <sensor_msgs/Joy.h>

ros::ServiceClient client;


void joystick_callback(const sensor_msgs::Joy::ConstPtr& joy)
{
  auto button_pressed = joy->buttons[0]; // 0 is the 'A' button on a wired xbox 360 controller
  if(button_pressed){
    ros_services::ImuValue srv;
    client.call(srv); 
    std::cout << "Got accel x: " << srv.response.current_x_orientation_s << std::endl;
  }
}


int main(int argc, char **argv)
{
        ros::init(argc,argv,"imu_client_node");
        ros::NodeHandle n;
        ros::NodeHandle nh_;
        ros::Subscriber joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, joystick_callback);
    client = n.serviceClient<ros_services::ImuValue>("imu_status_server");
    ros_services::ImuValue srv;
    client.call(srv);       
    std::cout << "Got accel x: " << srv.response.current_x_orientation_s << std::endl;

        if (client.call(srv))
      {
        ROS_INFO("Sum: %ld", (long int)srv.response.current_x_orientation_s);
      }
      else
      {
        ROS_ERROR("Failed to call service add_two_ints");
        return 1;
      }

  return 0;
}

the srv file

float64 current_x_orientation_c
---
float64 current_x_orientation_s
bool success

And the control launch file is here

<launch>
<arg name="namespace" default="thrbot"/>
<arg name="joy_id" default="0"/>
<arg name="axis_x" default="4"/>
<arg name="axis_y" default="3"/>
<arg name="axis_z" default="1"/>
<arg name="axis_yaw" default="0"/>

<arg name="gui_on" default="true"/>

<include file="$(find thrbot_control)/launch/start_thruster_manager.launch">
    <arg name="uuv_name" value="$(arg namespace)"/>
</include>

 <node name="joy_node" pkg="joy" type="joy_node">
 </node>

<group ns="$(arg namespace)">
    <rosparam file="$(find thrbot_control)/config/inertial.yaml" command="load"/>
    <rosparam file="$(find thrbot_control)/config/vel_pid_control.yaml" command="load"/>

    <node pkg="uuv_control_cascaded_pid" type="AccelerationControl.py" name="acceleration_control"
        output="screen">
        <param name="tf_prefix" type="string" value="$(arg namespace)/" />
    </node>

    <node pkg="uuv_control_cascaded_pid" type="VelocityControl.py" name="velocity_control"
        output="screen">
        <remap from="odom" to="pose_gt"/>
    </node>
</group>

<include file="$(find uuv_teleop)/launch/uuv_teleop.launch">
    <arg name="uuv_name" value="$(arg namespace)"/>
    <arg name="joy_id" value="$(arg joy_id)"/>
    <arg name="output_topic" value="cmd_vel"/>
    <arg name="message_type" value="twist"/>
    <arg name="axis_yaw" value="$(arg axis_yaw)"/>
    <arg name="axis_x" value="$(arg axis_x)"/>
    <arg name="axis_y" value="$(arg axis_y)"/>
    <arg name="axis_z" value="$(arg axis_z)"/>
    <arg name="gain_yaw" default="0.1"/>
    <arg name="gain_x" default="0.2"/>
    <arg name="gain_y" default="0.2"/>
    <arg name="gain_z" default="0.2"/>
    
    <!--arg name="gain_yaw" default="0.1"/> -->
    <!--arg name="gain_x" default="0.2"/>  -->
    <!--arg name="gain_y" default="0.2"/> -->
    <!--arg name="gain_z" default="0.2"/> -->
</include>

Any help?

Answer 1

Since you want to use a joystick input to trigger events in ros I would say to use the joy package and just add it to your launch file like so:

  <node name="joy_node" pkg="joy" type="joy_node">
  </node>

Then you just need to setup a subscriber in your client to handle joystick input and call the service appropriately like so

joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, joystick_callback, this);

and the callback

void joystick_callback(const sensor_msgs::Joy::ConstPtr& joy)
{
  auto button_pressed = joy->buttons[0]; // 0 is the 'A' button on a wired xbox 360 controller
  if(button_pressed){
    imu_service::ImuValue srv;
    client.call(srv); 
    std::cout << "Got accel x: " << srv.response.current_x_orientation_s << std::endl;
  }
}

Note that the above code assumes client is a global variable.