在Xvisio SDK安装目录下:/usr/share/ros-wrapper/demo_node
注意：关于ROS topic，service等相关概念请查阅此网站。
/xv_sdk/xv_dev/slam/start
消息体 std_srvs::Trigger：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/slam/start");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/slam/stop
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/slam/stop");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/slam/get_pose_at
消息体xv_sdk::GetPose(自定义服务消息体)：
# request
time timestamp
---
# response
geometry_msgs/PoseStamped pose
示例代码：
#include "xv_sdk/GetPoseAt.h"
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<xv_sdk::GetPoseAt>("/xv_sdk/xv_dev/slam/get_pose_at");
xv_sdk::GetPoseAt srv;
srv.request.timestamp = ros::Time(); //此处的时间可以是某个时刻fisheye 图像的时间戳
if (client.call(srv))
{
std::cout<< "x:"<<srv.response.pose.pose.orientation.x<< std::endl;
std::cout<< "y:"<<srv.response.pose.pose.orientation.y<< std::endl;
std::cout<< "z"<<srv.response.pose.pose.orientation.z<< std::endl;
...
}
}
/xv_sdk/xv_dev/slam/load_map_cslam
消息体LoadMapAndSwithcCslam(自定义服务消息体)：
string filename # request
---
bool success # response
string message
示例代码：
#include <xv_sdk/LoadMapAndSwithcCslam.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<xv_sdk::LoadMapAndSwithcCslam>("/xv_sdk/xv_dev/slam/load_map_cslam");
xv_sdk::LoadMapAndSwithcCslam srv;
srv.request.filename = "map.bin";
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/slam/save_map_cslam
消息体SaveMapAndSwitchCslam(自定义服务消息体)：
string filename # request
---
bool success # response
string message
示例代码：
#include "xv_sdk/SaveMapAndSwitchCslam.h"
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<xv_sdk::SaveMapAndSwithcCslam>("/xv_sdk/xv_dev/slam/save_map_cslam");
xv_sdk::SaveMapAndSwithcCslam srv;
srv.request.filename = "map.bin";
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/sgbm_camera/stop
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/sgbm_camera/stop");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/sgbm_camera/start
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/sgbm_camera/start");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/imu_sensor/start_orientation
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/imu_sensor/start_orientation");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/imu_sensor/stop_orientation
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/imu_sensor/stop_orientation");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
}
/xv_sdk/xv_dev/imu_sensor/get_orientation
消息体xv_sdk::GetOrientation：
duration prediction # request
---
OrientationStamped orientation # response
示例代码：
#include <xv_sdk/GetOrientation.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<xv_sdk::GetOrientation>("/xv_sdk/xv_dev/imu_sensor/get_orientation");
xv_sdk::GetOrientation srv;
srv.request.prediction = ros::Duration(0);
if (client.call(srv))
{
std::cout<< srv.response.orientation.angularVelocity.x <<std::endl;
...
}
}
注意：要先启动orientation
/xv_sdk/xv_dev/imu_sensor/get_orientation_at
消息体 xv_sdk::GetOrientationAt：
time timestamp # request
---
geometry_msgs/PoseStamped pose # response
示例代码：
#include <xv_sdk/GetOrientationAt.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<xv_sdk::GetOrientationAt>("/xv_sdk/xv_dev/imu_sensor/get_orientation_at");
xv_sdk::GetOrientationAt srv;
srv.request.timestamp = ros::Time(0);
if (client.call(srv))
{
std::cout<< srv.response.orientation.angularVelocity.x <<std::endl;
...
}
}
注意：要先启动orientation
/xv_sdk/xv_dev/color_camera/start
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/color_camera/start");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
...
}
/xv_sdk/xv_dev/color_camera/stop
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/color_camera/stop");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
...
}
/xv_sdk/xv_dev/tof_camera/start
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/tof_camera/start");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
...
}
/xv_sdk/xv_dev/tof_camera/stop
消息体 std_srvs::Trigger(ROS 标准服务消息体)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
示例代码：
#include <std_srvs/Trigger.h>
...
int main(int argc, char **argv)
{
ros::NodeHandle n;
...
ros::ServiceClient client = n.serviceClient<std_srvs::Trigger>("/xv_sdk/xv_dev/tof_camera/stop");
std_srvs::Trigger srv;
if (client.call(srv))
{
bool success = srv.response.success;
std::string message = srv.response.message;
}
...
}
/xv_sdk/xv_dev/slam/pose
消息体 xv_sdk::PoseStampedConfidence：
float64 confidence
geometry_msgs/PoseStamped poseMsg #ROS 标准Topic msg
示例代码：
#include "xv_sdk/PoseStampedConfidence.h"
...
void slamPoseCallback(const xv_sdk::PoseStampedConfidence& msg)
{
std::cout <<"slam pose confidence: " << msg.confidence<<std::endl;
std::cout <<"slam pose x: "<<msg.poseMsg.pose.position.x<<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
ros::Subscriber slamPoseSub = n.subscribe("/xv_sdk/xv_dev/slam/pose", 1000, slamPoseCallback);
...
ros::spin();
return 0;
}
注意：pose话题提供两种类型的消息结构，如果在xv_sdk.hpp头文件中定义了#define ENABLE_POSE_WITH_CONFIDENCE 则使用xv_sdk::PoseStampedConfidence消息体结构，如果没有定义则使用geometry_msgs::PoseStamped消息体结构订阅pose话题，代码如下：
消息体 geometry_msgs::PoseStamped（Ros标准消息体）：
std_msgs/Header header
geometry_msgs/Pose pose
示例代码：
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/String.h"
#include "rtk_info/rtk.h"
...
void slamPoseCallback(const geometry_msgs::PoseStamped& msg)
{
std::cout <<"slam pose x: " msg.pose.position.x<<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
ros::Subscriber slamPoseSub = n.subscribe("/xv_sdk/xv_dev/slam/pose", 1000, slamPoseCallback);
...
ros::spin();
return 0;
}
/xv_sdk/xv_dev/slam/trajectory
消息体 nav_msgs::Path(Ros 标准消息体)：
std_msgs/Header header
geometry_msgs/PoseStamped[] poses
示例代码：
...
#include "nav_msgs/Path.h"
...
void slamTrajectoryCallback(const nav_msgs::Path& msg)
{
std::cout <<"trajectory poses size:" <<msg.poses.size()<<std::endl;
std::cout <<"trajectory poses[0] x:"<<msg.poses[0].pose.position.x<<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber slamTrajectorySub = n.subscribe("/xv_sdk/xv_dev/slam/trajectory", 1000, slamTrajectoryCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅SLAM trajectory话题，需要在xv_sdk.h头文件中修改宏定义如下：
#define USE_SLAM_PATH
/xv_sdk/xv_dev/slam/stereo_planes
消息体 nav_msgs::Path(Ros 标准消息体)：
std_msgs/Header header
geometry_msgs/PoseStamped[] poses
示例代码：
...
#include "nav_msgs/Path.h"
...
void slamTrajectoryCallback(const nav_msgs::Path& msg)
{
std::cout <<"trajectory poses size:" <<msg.poses.size()<<std::endl;
std::cout <<"trajectory poses[0] x:"<<msg.poses[0].pose.position.x<<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber slamTrajectorySub = n.subscribe("/xv_sdk/xv_dev/slam/stereo_planes", 1000, slamTrajectoryCallback);
...
ros::spin();
return 0;
}
/xv_sdk/xv_dev/slam/stereo_planes
消息体 xv_sdk::Planes：
Plane[] planes
xv_sdk::Plane消息体:
# Frame this data is associated with
string frame_id
# Polygon describing the actual detected area
geometry_msgs/Point[] points
# Plane normal
geometry_msgs/Vector3 normal
# Plane signed distance to world frame origin (according to normal direction)
float64 d
# Plane unique identifier
string id
示例代码：
...
#include "xv_sdk/Planes.h"
#include "xv_sdk/Plane.h"
...
void slamStereoPlanesCallback(const xv_sdk::Planes & msg)
{
std::cout<<"slam stereo planes size: "<<msg.planes.size()<<std::endl;
std::cout<<"plane x: "<<msg.planes[0].points[0].x <<std::endl;
std::cout<<"plane y: "<<msg.planes[0].points[0].y <<std::endl;
std::cout<<"plane z: "<<msg.planes[0].points[0].z <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber slamStereoPlanes = n.subscribe("/xv_sdk/xv_dev/slam/stereo_planes", 1000, slamStereoPlanesCallback);
...
ros::spin();
return 0;
}
注意：
如果要需要订阅SLAM stereo_planes话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_FE
/xv_sdk/xv_dev/slam/tof_planes
消息体 xv_sdk::Planes：
Plane[] planes
xv_sdk::Plane消息体:
# Frame this data is associated with
string frame_id
# Polygon describing the actual detected area
geometry_msgs/Point[] points
# Plane normal
geometry_msgs/Vector3 normal
# Plane signed distance to world frame origin (according to normal direction)
float64 d
# Plane unique identifier
string id
示例代码：
...
#include "xv_sdk/Planes.h"
#include "xv_sdk/Plane.h"
...
void slamTofPlanesCallback(const xv_sdk::Planes & msg)
{
std::cout<<"slam tof planes size: "<<msg.planes.size()<<std::endl;
std::cout<<"plane x: "<<msg.planes[0].points[0].x <<std::endl;
std::cout<<"plane y: "<<msg.planes[0].points[0].y <<std::endl;
std::cout<<"plane z: "<<msg.planes[0].points[0].z <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber slamTofPlanes = n.subscribe("/xv_sdk/xv_dev/slam/tof_planes", 1000, slamTofPlanesCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅SLAM stereo_planes话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_TOF
/xv_sdk/xv_dev/sgbm_camera/image_sgbm
消息体 sensor_msgs::Image(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
示例代码：
...
#include "sensor_msgs/Image.h"
...
void slamSgbmImageCallback(const sensor_msgs::Image & msg)
{
std::cout<<"sgbm Image size: "<<msg.height * msg.width <<std::endl;
std::cout<<"image step: "<<msg.step <<std::endl;
std::cout<<"image encoding: "<< msg.encoding <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber sgbmImage = n.subscribe("/xv_sdk/xv_dev/sgbm_camera/image_sgbm", 1000, slamSgbmImageCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_SGBM
...
#define USE_SGBM_IMAGE
/xv_sdk/xv_dev/sgbm_camera/point_cloud
消息体 sensor_msgs::PointCloud2(ROS标准消息体)
std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense
示例代码：
...
#include "sensor_msgs/PointCloud2.h"
...
void slamSgbmPointCloudCallback(const sensor_msgs::PointCloud2 & msg)
{
std::cout<<"point cloud width: "<<msg.width <<std::endl;
std::cout<<"point cloud height: "<<msg.height <<std::endl;
std::cout<<"point cloud first data: "<<((float*)msg.data.data())[0] <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber sgbmPointCloud = n.subscribe("/xv_sdk/xv_dev/sgbm_camera/point_cloud", 1000, slamSgbmPointCloudCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_SGBM
...
#define USE_SGBM_POINTCLOUD
//定义点云数据的单位为米，如果注释则为毫米
#define SGBM_POINTCLOUD_UNIT_M
//点云是否转换为世界坐标系，根据需要打开
//#define CONVERT_TO_WORLD_COORDINATE_SYSTEM
//点云数据是由固件生成，这个要有特定的固件版本支持
//#define SGBM_FIRMWARE_CLOUD
//#define ENABLE_LESS_POINT_CLOUD
/xv_sdk/xv_dev/color_camera/image_color
消息体 sensor_msgs::Image(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
示例代码：
...
#include "sensor_msgs/Image.h"
...
void rgbImageCallback(const sensor_msgs::Image & msg)
{
std::cout<<"sgbm Image size: "<<msg.height * msg.width <<std::endl;
std::cout<<"image step: "<<msg.step <<std::endl;
std::cout<<"image encoding: "<< msg.encoding <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber rgbCameraImage = n.subscribe("/xv_sdk/xv_dev/color_camera/image_color", 1000, rgbImageCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_RGB
/xv_sdk/xv_dev/color_camera/camera_info
消息体 sensor_msgs::CameraInfo(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string distortion_model
float64[] D
float64[9] K
float64[9] R
float64[12] P
uint32 binning_x
uint32 binning_y
sensor_msgs/RegionOfInterest roi
示例代码：
...
#include "sensor_msgs/CameraInfo.h"
...
void rgbCameraInfoCallback(const sensor_msgs::CameraInfo & msg)
{
std::cout<<"rgb camera info width: "<<msg.width <<std::endl;
std::cout<<"rgb camera info height: "<<msg.height <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber rgbCameraInfo = n.subscribe("/xv_sdk/xv_dev/color_camera/camera_info", 1000, rgbCameraInfoCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_RGB
/xv_sdk/xv_dev/fisheye_cameras/left/image
消息体 sensor_msgs::Image(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
示例代码：
...
#include "sensor_msgs/Image.h"
...
void rgbImageCallback(const sensor_msgs::Image & msg)
{
std::cout<<"sgbm Image size: "<<msg.height * msg.width <<std::endl;
std::cout<<"image step: "<<msg.step <<std::endl;
std::cout<<"image encoding: "<< msg.encoding <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber fisheyeLeftCameraImage = n.subscribe("/xv_sdk/xv_dev/fisheye_cameras/left/image", 1000, fisheyeLeftImageCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_FE
/xv_sdk/xv_dev/fisheye_cameras/left/camera_info
消息体 sensor_msgs::CameraInfo(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string distortion_model
float64[] D
float64[9] K
float64[9] R
float64[12] P
uint32 binning_x
uint32 binning_y
sensor_msgs/RegionOfInterest roi
示例代码：
...
#include "sensor_msgs/CameraInfo.h"
...
void fisheyeLeftCameraInfoCallback(const sensor_msgs::CameraInfo & msg)
{
std::cout<<"rgb camera info width: "<<msg.width <<std::endl;
std::cout<<"rgb camera info height: "<<msg.height <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber fisheyeLeftCameraImage = n.subscribe("/xv_sdk/xv_dev/fisheye_cameras/left/image", 1000, fisheyeLeftImageCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_FE
/xv_sdk/xv_dev/fisheye_cameras/right/image
/xv_sdk/xv_dev/fisheye_cameras/right/camera_info
注意：鱼眼相机的右眼相机的实现请参考上面的鱼眼相机的左眼相机的实现。
/xv_sdk/xv_dev/imu_sensor/data_raw
消息体 sensor_msgs::Imu(ROS标准消息体)：
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance
示例代码：
...
#include "sensor_msgs/Imu.h"
...
void imuRawDataCallback(const sensor_msgs::Imu& msg)
{
std::cout<<"imu acceleration x: "<<msg.linear_acceleration.x <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber ImuRawData = n.subscribe("/xv_sdk/xv_dev/imu_sensor/data_raw", 1000, imuRawDataCallback);
...
ros::spin();
return 0;
}
/xv_sdk/xv_dev/imu_sensor/orientation
消息体 xv_sdk::OrientationStamped：
std_msgs/Header header
float64[9] matrix
geometry_msgs/Quaternion quaternion
geometry_msgs/Vector3 angularVelocity
示例代码：
...
#include "xv_sdk/OrientationStamped.h"
...
void imuOrientationCallback(const xv_sdk::OrientationStamped& msg)
{
std::cout<<"imu quaternion x: "<<msg.quaternion.x <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber ImuOrientationData = n.subscribe("/xv_sdk/xv_dev/imu_sensor/orientation", 1000, imuOrientationCallback);
...
ros::spin();
return 0;
}
/xv_sdk/xv_dev/tof_camera/camera_info
消息体 sensor_msgs::CameraInfo(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string distortion_model
float64[] D
float64[9] K
float64[9] R
float64[12] P
uint32 binning_x
uint32 binning_y
sensor_msgs/RegionOfInterest roi
示例代码：
...
#include "sensor_msgs/CameraInfo.h"
...
void tofCameraInfoCallback(const sensor_msgs::CameraInfo & msg)
{
std::cout<<"tof camera info width: "<<msg.width <<std::endl;
std::cout<<"tof camera info height: "<<msg.height <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber tofCameraInfo = n.subscribe("/xv_sdk/xv_dev/tof_camera/camera_info", 1000, tofCameraInfoCallback);
...
ros::spin();
return 0;
}
注意：如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_TOF
/xv_sdk/xv_dev/tof_camera/image
消息体 sensor_msgs::Image(ROS标准消息体)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
示例代码：
...
#include "sensor_msgs/Image.h"
...
void tofPointCloudCallback(const sensor_msgs::PointCloud2 & msg)
{
std::cout<<"tof point cloud width: "<<msg.width <<std::endl;
std::cout<<"tof point cloud height: "<<msg.height <<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber tofImage = n.subscribe("/xv_sdk/xv_dev/tof_camera/image", 1000, tofImageCallback);
...
ros::spin();
return 0;
}
注意：Tof image中深度数据的单位为米，数据类型为float型。如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_TOF
/xv_sdk/xv_dev/tof_camera/point_cloud
消息体 sensor_msgs::PointCloud2(ROS标准消息体)
std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense
示例代码：
...
#include "sensor_msgs/PointCloud2.h"
...
struct TofPointCloud
{
float x;
float y;
float z;
};
#pragma pack()
void tofPointCloudCallback(const sensor_msgs::PointCloud2 & msg)
{
std::cout<<"tof point cloud width: "<<msg.width <<std::endl;
std::cout<<"tof point cloud height: "<<msg.height <<std::endl;
TofPointCloud* pointCloudPtr = (TofPointCloud *)msg.data.data();
std::cout<<"First point x:"<<pointCloudPtr[0].x<<std::endl;
std::cout<<"First point y:"<<pointCloudPtr[0].y<<std::endl;
std::cout<<"First point z:"<<pointCloudPtr[0].z<<std::endl;
}
...
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo");
...
ros::NodeHandle n;
...
ros::Subscriber tofPointCloud = n.subscribe("/xv_sdk/xv_dev/tof_camera/point_cloud", 1000, tofPointCloudCallback);
...
ros::spin();
return 0;
}
注意：Tof 点云中的数据类型为float。如果要需要订阅此话题，需要在xv_sdk.h头文件中修改宏定义如下：
//#define NOT_USE_TOF
...
//#define TOF_PC_WITH_RGB