← ROS1
← Xvisio SDK Documentation Home Page
In Xvisio SDK installation path: /usr/share/ros-wrapper/demo_node
Note：Click here for related concepts of ROS topic and service.
/xv_sdk/xv_dev/slam/start
message body std_srvs::Trigger：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code:
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body xv_sdk::GetPose (customized service message body)：
# request
time timestamp
---
# response
geometry_msgs/PoseStamped pose
sample code：
#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(); //the time here indicates sometime timestemp of fisheye camera
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
message body LoadMapAndSwithcCslam (customized service message body)：
string filename # request
---
bool success # response
string message
sample code：
#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
message bodySaveMapAndSwitchCslam (customized service message body)：
string filename # request
---
bool success # response
string message
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body xv_sdk::GetOrientation：
duration prediction # request
---
OrientationStamped orientation # response
sample code：
#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;
...
}
}
Note：start orientation first.
/xv_sdk/xv_dev/imu_sensor/get_orientation_at
message body xv_sdk::GetOrientationAt：
time timestamp # request
---
geometry_msgs/PoseStamped pose # response
sample code：
#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;
...
}
}
Note：start orientation first.
/xv_sdk/xv_dev/color_camera/start
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body std_srvs::Trigger (ROS standard service message body)：
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
sample code：
#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
message body xv_sdk::PoseStampedConfidence：
float64 confidence
geometry_msgs/PoseStamped poseMsg #ROS 标准Topic msg
sample code：
#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;
}
Note：pose topic provides two kinds of message structure. xv_sdk::PoseStampedConfidence can be used if #define ENABLE_POSE_WITH_CONFIDENCE has been defined in head file xv_sdk.hpp. Otherwise, use geometry_msgs::PoseStamped to subscribe pose topic. Code is shown as below:
message body geometry_msgs::PoseStamped（Ros standard service message body）：
std_msgs/Header header
geometry_msgs/Pose pose
sample code：
#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
message body nav_msgs::Path(Ros standard service message body)：
std_msgs/Header header
geometry_msgs/PoseStamped[] poses
sample code:
...
#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;
}
Note：Modify macro definition in head file xv_sdk.h as below to subscribe SLAM trajectory topic:
#define USE_SLAM_PATH
/xv_sdk/xv_dev/slam/stereo_planes
message body nav_msgs::Path (Ros standard service message body)：
std_msgs/Header header
geometry_msgs/PoseStamped[] poses
sample code：
...
#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
message body xv_sdk::Planes：
Plane[] planes
xv_sdk::Plane message body:
# 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
sample code：
...
#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;
}
Note：
Modify macro definition in head file as below to subscribe SLAM stereo_planes topic:
//#define NOT_USE_FE
/xv_sdk/xv_dev/slam/tof_planes
message body xv_sdk::Planes：
Plane[] planes
xv_sdk::Plane message body:
# 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
code sample：
...
#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;
}
Note：Modify macro definition as bleow in head file xv_sdk.h to subscribe SLAM stereo_planes topic:
//#define NOT_USE_TOF
/xv_sdk/xv_dev/sgbm_camera/image_sgbm
message body sensor_msgs::Image(ROS standard message body)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
code sample：
...
#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;
}
Note：modify macro definition in head file as below to subscribe this topic:
//#define NOT_USE_SGBM
...
#define USE_SGBM_IMAGE
/xv_sdk/xv_dev/sgbm_camera/point_cloud
message body sensor_msgs::PointCloud2(ROS standard message body)
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
sample code：
...
#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;
}
Note：Modify macro definition as bleow in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_SGBM
...
#define USE_SGBM_POINTCLOUD
//the unit of point cloud data is meter, once noted, the unit is millimeter.
#define SGBM_POINTCLOUD_UNIT_M
//whether transfer point cloud to world coordinate system
//#define CONVERT_TO_WORLD_COORDINATE_SYSTEM
//point cloud is generated by particular firmware.
//#define SGBM_FIRMWARE_CLOUD
//#define ENABLE_LESS_POINT_CLOUD
/xv_sdk/xv_dev/color_camera/image_color
message body sensor_msgs::Image(ROS standard message body)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
sample code：
...
#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;
}
Note：Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_RGB
/xv_sdk/xv_dev/color_camera/camera_info
message body sensor_msgs::CameraInfo (ROS standard message body)：
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
sample code：
...
#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;
}
Note：Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_RGB
/xv_sdk/xv_dev/fisheye_cameras/left/image
message body sensor_msgs::Image (ROS standard message body)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
sample code：
...
#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;
}
Note：Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_FE
/xv_sdk/xv_dev/fisheye_cameras/left/camera_info
message body sensor_msgs::CameraInfo(ROS standard message body)：
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
sample code：
...
#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;
}
Note：Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_FE
/xv_sdk/xv_dev/fisheye_cameras/right/image
/xv_sdk/xv_dev/fisheye_cameras/right/camera_info
Note：For right eye camera,please refer to the implementation of the left eye camera of the fish eye camera above.
/xv_sdk/xv_dev/imu_sensor/data_raw
message body sensor_msgs::Imu (ROS standard message body)：
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
sample code：
...
#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
message body xv_sdk::OrientationStamped：
std_msgs/Header header
float64[9] matrix
geometry_msgs/Quaternion quaternion
geometry_msgs/Vector3 angularVelocity
sample code：
...
#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
message body sensor_msgs::CameraInfo (ROS standard message body)：
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
sample code：
...
#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;
}
Note：Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_TOF
/xv_sdk/xv_dev/tof_camera/image
message body sensor_msgs::Image (ROS standard message body)：
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
sample code：
...
#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;
}
Note：The unit of depth data in Tof image is meter, and the data type is float. Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_TOF
/xv_sdk/xv_dev/tof_camera/point_cloud
message body sensor_msgs::PointCloud2 (ROS standard message body)
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
sample code：
...
#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;
}
Note：The data type of Tof point cloud is float. Modify macro definition as below in head file xv_sdk.h to subscribe this topic:
//#define NOT_USE_TOF
...
//#define TOF_PC_WITH_RGB