executive_usarsim and teleop issue

edit

I wish to interface ROS-USARSim. and using a interface package executive_usarsim.

It spawn P3AT in USARSim but while running rosrun teleop_twist_keyboard teleop_twist_keyboard.py

P3AT do not move.

Actually in rqt_graph; executive_usarsim interface donot subscribe twist messages;

Therefore i wish to write a cpp node which can subscribe /cmd_vel topic and publish it on /executive_usarsim/status topic.

please help how to write it in a single cpp file.

Here is rosnode info executive_usarsim

 cr-lab-tu@crlabtu-HP-Compaq-8100-Elite-SFF-PC:~$ rosnode info executive_usarsim
 --------------------------------------------------------------------------------
 Node [/executive_usarsim]
 Publications: 
  * /executive_usarsim/status [std_msgs/String]
  * /rosout [rosgraph_msgs/Log]

 Subscriptions: 
  * /executive_usarsim/status [std_msgs/String]

 Services: 
  * /executive_usarsim/get_loggers
  * /executive_usarsim/set_logger_level


 contacting node http://crlabtu-HP-Compaq-8100-Elite-SFF-PC:45055/ ...
 Pid: 12463
 Connections:
  * topic: /executive_usarsim/status
     * to: /executive_usarsim
     * direction: outbound
     * transport: TCPROS
  * topic: /rosout
     * to: /rosout
     * direction: outbound
     * transport: TCPROS
  * topic: /executive_usarsim/status
     * to: /executive_usarsim (http://crlabtu-HP-Compaq-8100-Elite-SFF-PC:45055/)
     * direction: inbound
     * transport: TCPROS

and rosnode info teleop_twist_keyboard

 cr-lab-tu@crlabtu-HP-Compaq-8100-Elite-SFF-PC:~$ rosnode info teleop_twist_keyboard
 --------------------------------------------------------------------------------
 Node [/teleop_twist_keyboard]
 Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /cmd_vel [geometry_msgs/Twist]

 Subscriptions: None

 Services: 
 * /teleop_twist_keyboard/set_logger_level
 * /teleop_twist_keyboard/get_loggers


 contacting node http://crlabtu-HP-Compaq-8100-Elite-SFF-PC:55529/ ...
 Pid: 12620
 Connections:
  * topic: /rosout
      * to: /rosout
      * direction: outbound
      * transport: TCPROS

EDIT1:

here is the code i have written to solve the above issue,

  #include <ros/ros.h>
 #include "std_msgs/String.h"
 #include <sstream>
 class SubscribeAndPublish
 {
 public:
   SubscribeAndPublish()
   {
     //Topic you want to publish
     pub_ = n_.advertise<std_msgs::String>("/executive_usarsim", 1);

     //Topic you want to subscribe
     sub_ = n_.subscribe("/cmd_vel", 1, &SubscribeAndPublish::callback, this);
   }

   void callback(const std_msgs::String::ConstPtr& input)
   {
     std_msgs::String output;
     //output=input;
     //.... do something with the input and generate the output...
     pub_.publish(input);
   }

 private:
   ros::NodeHandle n_; 
   ros::Publisher pub_;
   ros::Subscriber sub_;

 };//End of class SubscribeAndPublish

 int main(int argc, char **argv)
 {
   //Initiate ROS
   ros::init(argc, argv, "sub-pub");

   //Create an object of class SubscribeAndPublish that will take care of everything
   SubscribeAndPublish SAPObject;

   ros::spin();

   return 0;
 }

But it is only publishing and not subscribing.

bcrlab@bcrlab-HP-Compaq-8100-Elite-SFF-PC:~$ rosnode info sub_pub
--------------------------------------------------------------------------------
Node [/sub_pub]
Publications: 
 * /executive_usarsim/status [std_msgs/String]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /cmd_vel [geometry_msgs/Twist]

Services: 
 * /sub_pub/set_logger_level
 * /sub_pub/get_loggers


contacting node http://bcrlab-HP-Compaq-8100-Elite-SFF-PC:33082/ ...
Pid: 21939
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /executive_usarsim/status
    * to: /executive_usarsim
    * direction: outbound
    * transport: TCPROS

and

bcrlab@bcrlab-HP-Compaq-8100-Elite-SFF-PC:~$ rostopic info \cmd_vel
Type: geometry_msgs/Twist

Publishers: 
 * /teleop_twist_keyboard (http://bcrlab-HP-Compaq-8100-Elite-SFF-PC:45082/)

Subscribers: 
 * /sub_pub (http://bcrlab-HP-Compaq-8100-Elite-SFF-PC:33082/)

EDIT2:

Here is the updated code which work for me,

#include <ros/ros.h>
#include "std_msgs/String.h"
#include <sstream>
#include <geometry_msgs/Twist.h>
class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    //Topic you want to publish
    pub_ = n_.advertise<geometry_msgs::Twist>("/executive_usarsim/pub_tele", 1);

    //Topic you want to subscribe
    sub_ = n_.subscribe("/cmd_vel", 1, &SubscribeAndPublish::callback, this);
  }

  void callback(const geometry_msgs::Twist::ConstPtr& input)
  {
    geometry_msgs::Twist output;
    //output=input;
    //.... do something with the input and generate the output...
    pub_.publish(input);
  }

private:
  ros::NodeHandle n_; 
  ros::Publisher pub_;
  ros::Subscriber sub_;

};//End of class SubscribeAndPublish

int main(int argc, char **argv)
{
  //Initiate ROS
  ros::init(argc, argv, "sub_pub");

  //Create an object of class SubscribeAndPublish that will take care of everything
  SubscribeAndPublish SAPObject;

  ros::spin();

  return 0;
}

But now the issue is ... (more)