test_custom_particles.cpp
// // Created by gary on 2019/8/27. // #include <ros/ros.h> #include <std_msgs/String.h> #include <geometry_msgs/PoseArray.h> #include <geometry_msgs/Pose.h> #include "amcl/amcl_particles.h" #include <stdlib.h> ros::NodeHandle *nh = 0; ros::ServiceClient test_custom_particles_; int main(int argc, char** argv) { ros::init(argc, argv, "test_custom_particles"); ros::NodeHandle nh_("~"); nh = &nh_; while(!ros::service::waitForService("/amcl/set_particles", ros::Duration(3.0))) { ROS_ERROR("The service of /amcl/set_particles is not available."); } test_custom_particles_ = nh->serviceClient<amcl::amcl_particles>("/amcl/set_particles", true); if(!test_custom_particles_) { ROS_ERROR("Could not initialize goal_reached_client service from %s",test_custom_particles_.getService().c_str()); //抛出错误，以及其他处理机制 return 0; } ROS_INFO("11111"); amcl::amcl_particles test_custorm_particle_srv; test_custorm_particle_srv.request.pose_array_msg.header.stamp = ros::Time::now();//-1意思为告诉用户到达零点失败 //test_custorm_particle_srv.request.pose_array_msg.poses = (geometry_msgs::Pose *)malloc(sizeof(geometry_msgs::Pose) * 1000); geometry_msgs::Pose le; for(int i = 0; i < 1000; i++) { le.position.x = 0; le.position.y = 0; le.position.z = 0; le.orientation.x = 0; le.orientation.y = 0; le.orientation.z = 0; le.orientation.w = 1; test_custorm_particle_srv.request.pose_array_msg.poses.push_back(le) ; } ROS_INFO("2222"); if(test_custom_particles_.call(test_custorm_particle_srv)) { if(test_custorm_particle_srv.response.success) { ROS_INFO("success."); } else { ROS_ERROR("failture."); } } ros::spin(); return 0; }
amcl_particles.srv
geometry_msgs/PoseArray pose_array_msg --- bool success