VEXProMotor13Subscribe.cpp
Go to the documentation of this file.
1 /*
2  * VEXProMotor13Subscribe.cpp
3  * Control motor 13 speed by publishing the desired speed on a ros topic with e.g.
4  * $ rostopic pub my_topic std_msgs/Int32 120
5  */
6 
7 #include <ros.h>
8 #include <std_msgs/Int32.h>
9 #include <stdio.h>
10 #include "qemotoruser.h"
11 
12 /*
13  * Control motor 13 speed by publishing the desired speed on a ros topic with e.g.
14  * $ rostopic pub my_topic std_msgs/Int32 120
15  * The range of speeds is -255 to +255 (corresponding to full reverse to full forward).
16  * Publish negative speeds using the syntax below:
17  * $ rostopic pub my_topic std_msgs/Int32 -- -120
18  * (This construct tells the shell to feed everything after -- directly to rostopic.)
19  */
20 
22 CQEMotorUser &motor = CQEMotorUser::GetRef();
23 char *rosSrvrIp = "192.168.11.9";
24 
25 void messageCb(const std_msgs::Int32& motor13_msg){
26  int speed = motor13_msg.data;
27  printf("Received subscribed motor speed %d\n", speed);
28  motor.SetPWM(0, speed);
29 }
31 
32 
33 int main()
34 {
35  //nh.initNode();
36  nh.initNode(rosSrvrIp);
37  nh.subscribe(sub);
38 
39  while(1) {
40  sleep(1);
41  nh.spinOnce();
42  }
43 }
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int main()
ros::Subscriber< std_msgs::Int32 > sub("motor13", messageCb)
ros::NodeHandle nh
void messageCb(const std_msgs::Int32 &motor13_msg)
char * rosSrvrIp
CQEMotorUser & motor


rosserial_embeddedlinux
Author(s): Paul Bouchier
autogenerated on Mon Jun 10 2019 14:53:23