rosserial_qt_win/main.cpp at master · dennn66/rosserial_qt_win · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
/*
 *
 *
 *
 */


#include <QCoreApplication>
#include "ros.h"
#include <geometry_msgs/Twist.h>
#include <windows.h>

double dX = 0;
double dTh = 0;

void messageCb(const geometry_msgs::Twist& msg){

      printf("Recieved linear.x %f\n", msg.linear.x);

}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    ros::NodeHandle nh;
    char *ros_master = "10.0.2.4";
    printf("Connecting to server at %s\n", ros_master);
    nh.initNode(ros_master);


    printf("Advertising cmd_vel message\n");
    geometry_msgs::Twist twist_msg;
    ros::Publisher cmd_vel_pub("r1/cmd_vel", &twist_msg);
    ros::Subscriber<geometry_msgs::Twist> sub("r1/cmd_vel", &messageCb);
    nh.advertise(cmd_vel_pub);
    nh.subscribe(sub);


    printf("Go robot go!\n");
    while (1)
    {
        dX++;
        twist_msg.linear.x = dX;
        twist_msg.linear.y = 0;
        twist_msg.linear.z = 0;
        twist_msg.angular.x = 0;
        twist_msg.angular.y = 0;
        twist_msg.angular.z = dTh;
        cmd_vel_pub.publish(&twist_msg);

        nh.spinOnce();
        Sleep(100);
        printf(".");
    }
    return a.exec();
}