How to make a c++ node Listen and Publish
I feel like this should be a simple problem.
The listening callback function is working and it is printing in terminal but the publishing doesn't seem to be working. The node shows up in rostopic list but it does not echo anything.
The script is getting an observation from an APPL node and gets an action back. I then want to pass the action to the action node.
Ideally, I want to get something like this working as the published attribute:
void sendAction(int action)
{
printf("SENDING ACTION\n");
ros::NodeHandle nh;
ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
geometry_msgs::Twist vel;
vel.angular.z = 0;
vel.linear.x = 1;
vel_pub.publish(vel);
}
This is the whole script at the moment
using namespace std;
// Messege recieved callback
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
// Variables
std_msgs::String actionstr;
std::stringstream ss;
// Set up
ros::NodeHandle n;
// Publish and advertise
ros::Publisher chatter_pubs = n.advertise<std_msgs::String>("actionstr", 1000);
ros::Rate loop_rate(10);
// APPL NODE SHIZ
appl::appl_request srv;
ros::ServiceClient client = n.serviceClient<appl::appl_request>("appl_request");
//Send to POMDP Node
srv.request.cmd=2;
srv.request.obs=msg->data.c_str();
client.call(srv);
int action=srv.response.action;
// Get action string
if (action == 0) {ss << "Listen";}
if (action == 1) {ss << "OpenRight";}
if (action == 2) {ss << "OpenLeft";}
// Publish String
actionstr.data = ss.str();
chatter_pubs.publish(actionstr);
printf("Sent\n");
printf("Obs: %s Action: %d - %s\n", msg->data.c_str(), action, actionstr.data.c_str());
}
/* ------------------------------------------------ */
/* ---------------------******--------------------- */
/* ---------------------*MAIN*--------------------- */
/* ---------------------******--------------------- */
/* ------------------------------------------------ */
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
// Set to Appl client
ros::ServiceClient client = n.serviceClient<appl::appl_request>("appl_request");
srand((unsigned)time(0));
int tiger;//0=left 1=right
tiger=rand()%2;
// Reset Appl Controller.
appl::appl_request srv;
srv.request.cmd=1; //reset the controller first
client.call(srv);
int action=srv.response.action;
ROS_INFO("I Listen");
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
ros::spin();
return 0;
}