0

我创建了将 IMU 传感器值从服务器传递到客户端的 ROS 服务。我能够构建包，但有些客户端值全为零。

所以我想创建 IMU ROS 服务。我有一个服务器（在我的例子中是一个微控制器 ESC32），它可以获得 IMU 读数，并且在请求进一步处理时应该将 IMU 数据传递给客户端（在我的例子中是 Raspberry PI 4B）。所以我只需要传递原始数据IMU 数据。这是我的 .srv 文件

float64 x_orient_in
float64 y_orient_in
float64 z_orient_in
float64 w_orient_in
float64 x_veloc_in
float64 y_veloc_in
float64 z_veloc_in
float64 x_accel_in
float64 y_accel_in
float64 z_accel_in
---
float64 x_orient_out
float64 y_orient_out
float64 z_orient_out
float64 w_orient_out
float64 x_veloc_out
float64 y_veloc_out
float64 z_veloc_out
float64 x_accel_out
float64 y_accel_out
float64 z_accel_out
bool success

然后我的服务器 cpp 节点

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


bool get_val(ros_services::ImuValue::Request  &req, ros_services::ImuValue::Response &res)
{
    
    ROS_INFO("sending back response");

    res.current_x_orientation = get_orientation_x(); 
   
    
}


void imuCallback(const  sensor_msgs::Imu::ConstPtr& msg)
{
  
     double current_x_orientation;
     current_x_orientation= msg->orientation.x;
    
}



int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_status_server");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("imu_data", 1000, imuCallback);

  ros::ServiceServer 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>

int main(int argc, char **argv)
{
        ros::init(argc,argv,"imu_client_node");
        ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<ros_services::ImuValue>("imu_status_server");
    ros_services::ImuValue srv;
    client.call(srv);
    std::cout << "Got orient x: " << srv.response.x_orient_out << std::endl;
    std::cout << "Got orient y: " << srv.response.y_orient_out << std::endl;
    std::cout << "Got orient z: " << srv.response.z_orient_out << std::endl;
    std::cout << "Got orient w: " << srv.response.w_orient_out << std::endl;
    std::cout << "Got veloc x: " << srv.response.x_veloc_out << std::endl;
    std::cout << "Got veloc y: " << srv.response.y_veloc_out << std::endl;
    std::cout << "Got veloc z: " << srv.response.z_veloc_out << std::endl;
    std::cout << "Got accel x: " << srv.response.x_accel_out << std::endl;
    std::cout << "Got accel y: " << srv.response.y_accel_out << std::endl;
    std::cout << "Got accel z: " << srv.response.z_accel_out << std::endl;

  return 0;
}

IMU主题是

header: 
  seq: 93931
  stamp: 
    secs: 1919
    nsecs: 264000000
  frame_id: "thrbot/imu_link"
orientation: 
  x: 0.011051219171
  y: 0.0185614347587
  z: 0.271966050405
  w: -0.962064348743
orientation_covariance: [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001]
angular_velocity: 
  x: 0.000226259345086
  y: 0.0135818332253
  z: -0.0254669231582
angular_velocity_covariance: [1.1519236000000001e-07, 0.0, 0.0, 0.0, 1.1519236000000001e-07, 0.0, 0.0, 0.0, 1.1519236000000001e-07]
linear_acceleration: 
  x: 0.40810519334
  y: -0.109897852778
  z: 9.79053552816
linear_acceleration_covariance: [1.6e-05, 0.0, 0.0, 0.0, 1.6e-05, 0.0, 0.0, 0.0, 1.6e-05]

有什么帮助吗？

4

1 回答 1

1

服务器只是返回req上面代码中的值是从 传入的默认服务消息main()。当 IMU 值进入某个主题（在服务器代码中）时，您需要缓存它们。然后当服务被调用时，它需要从最后一个 IMU 值中提取。您的服务器需要包含以下内容：

void imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
    current_x_orientation = msg->orientation.x; //Variable declared somewhere in a scope the service can see
    //Save other needed fields here
}

在您的服务中，将每个response字段分配给保存在回调中的相应值。

于 2021-09-13T14:45:13.637 回答