在Gazebo仿真中,如何在每次按下操纵杆来控制UUV车辆时,调用一个带有启动文件的ROS服务?

2022-06-06 00:00:00 ros gazebo-simu c++

我正在使用Gazebo 9和ROS Melotic进行一些UUV模拟。我创建了一个ROS服务,将IMU传感器的值从服务器传递到客户端。现在,我想在每次控制UUV(即每次我按下操纵杆)时,将这项服务作为传感器反馈给控制器。下面是服务器和客户端节点以及.srv文件

服务器

#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;
}

客户端在此

#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;
}

srv文件

float64 current_x_orientation_c
---
float64 current_x_orientation_s
bool success

控件启动文件在此处

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

有帮助吗?


解决方案

由于您希望使用操纵杆输入来触发ros中的事件,因此我建议您使用joy package并将其添加到启动文件中,如下所示:

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

然后您只需要在您的客户端中设置一个订阅者来处理操纵杆输入并适当地调用服务,如下所示

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

和回调

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;
  }
}

请注意,上面的代码假定client是全局变量。

相关文章