使用类实现在回调函数中发布消息
#include <ros/ros.h>
#include "circle/PersonMsg.h"
#include "circle/velCommand.h"
#include <turtlesim/Spawn.h>
#include<geometry_msgs/Twist.h>
using namespace std;
#include <iostream>
using namespace std;
class SubscribeAndPublish
{
public:
string newName;
SubscribeAndPublish()
{
//Topic you want to publish
// pub_ = n_.advertise<msg_type>("/published_topic", 1);
//Topic you want to subscribe
sub_ = n_.subscribe("/add_turtle", 1, &SubscribeAndPublish::newCallback, this);
// sub_ = n_.subscribe("/velocity_command", 1, &SubscribeAndPublish::velCallback, this);
}
void newCallback(const circle::PersonMsg::ConstPtr& msg){
newName = msg->name;
// ROS_INFO("Turtle name: x: %s", msg->name.c_str());
ros::service::waitForService("/spawn");
ros::ServiceClient newTurtle_client = n_.serviceClient<turtlesim::Spawn>("/spawn");
// 初始化请求数据
turtlesim::Spawn srv;
srv.request.name = newName;
srv.request.x = 2.0;
srv.request.y = 2.0;
// ROS_INFO("name: %s, x: %0.2f, y: %0.2f", srv.request.name.c_str(), srv.request.x,srv.request.y);
newTurtle_client.call(srv);
}
// void velCallback(const circle::velCommand::ConstPtr& msg){
// if(newName == msg->name&&msg->on == true){
// string rest = "/cmd_vel";
// string topic = newName+rest;
// pub_ = n_.advertise<geometry_msgs::Twist>(topic, 10);
// ros::Rate loop_rate(1);
// while (ros::ok())
// {
// // 初始化learning_communication::Person类型的消息
// geometry_msgs::Twist vel_msg;
// vel_msg.linear.x = 2;
// vel_msg.angular.z = 0.5;
// // 发布消息
// pub_.publish(vel_msg);
// // ROS_INFO("vel: %2f, angel: %2f",vel_msg.linear.x,vel_msg.angular.z );
// // 按照循环频率延时
// loop_rate.sleep();
// }
// }
// // ROS_INFO("Turtle name: x: %s", msg->name.c_str());
// }
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::Subscriber sub_;
};
int main(int argc, char **argv){
// 初始化
ros::init(argc,argv,"newSub");
SubscribeAndPublish SAPObject;
// // 创建句柄
// ros::NodeHandle n;
// // 创建sub
// ros::Subscriber newSub = n.subscribe("/add_turtle", 10 ,newCallback);
ros::spin();
// 连接名为spawn的服务,创造client对象
// while (newName!=nullptr)
// {
// ros::service::waitForService("/spawn");
// ros::ServiceClient newTurtle_client = n.serviceClient<turtlesim::Spawn>("/spawn");
// // 初始化请求数据
// turtlesim::Spawn srv;
// srv.request.name = newName;
// srv.request.x = 2.0;
// srv.request.y = 2.0;
// // 请求服务调用
// ROS_INFO("name: %s, x: %0.2f, y: %0.2f", srv.request.name.c_str(), srv.request.x,srv.request.y);
// newTurtle_client.call(srv);
// // 显示服务结果
// ROS_INFO("Successful Response. Name: %s, x: %0.2f, y: %0.2f", srv.request.name.c_str(), srv.request.x,srv.request.y);
return 0;
// }
}