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
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
/*
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <px4.h>
#include "subscriber_params.h"
using namespace px4;
/**
* This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
*/
void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
}
void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) {
PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid);
}
class RCHandler {
public:
void callback(const PX4_TOPIC_T(rc_channels) &msg) {
PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid);
}
};
RCHandler rchandler;
namespace px4
{
bool task_should_exit = false;
}
PX4_MAIN_FUNCTION(subscriber) {
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of px4::init() before using any other
* part of the PX4/ ROS system.
*/
px4::init(argc, argv, "listener");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
px4::NodeHandle n;
/* Define parameters */
px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV);
px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF);
/* Read the parameter back for testing */
int32_t sub_interval;
float test_float;
PX4_PARAM_GET(p_sub_interv, &sub_interval);
PX4_INFO("Param SUB_INTERV = %d", sub_interval);
PX4_PARAM_GET(p_test_float, &test_float);
PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float);
/**
* The subscribe() call is how you tell ROS that you want to receive messages
* on a given topic. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. Messages are passed to a callback function, here
* called chatterCallback. subscribe() returns a Subscriber object that you
* must hold on to until you want to unsubscribe. When all copies of the Subscriber
* object go out of scope, this callback will automatically be unsubscribed from
* this topic.
*
* The second parameter to the subscribe() function is the size of the message
* queue. If messages are arriving faster than they are being processed, this
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval);
PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
PX4_INFO("subscribed");
/**
* px4::spin() will enter a loop, pumping callbacks. With this version, all
* callbacks will be called from within this thread (the main one). px4::spin()
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
*/
n.spin();
PX4_INFO("finished, returning");
return 0;
}
|
