-
-
Notifications
You must be signed in to change notification settings - Fork 74
/
Copy pathros-noetic-mavros.win.patch
239 lines (209 loc) · 6.75 KB
/
ros-noetic-mavros.win.patch
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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed6ff0806..594ca07c6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,6 +25,10 @@ find_package(catkin REQUIRED COMPONENTS
trajectory_msgs
)
+if(MSVC)
+ add_compile_options(/permissive-)
+endif()
+
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
@@ -44,7 +48,9 @@ find_package(GeographicLib REQUIRED)
## Check if the datasets are installed
include(CheckGeographicLibDatasets)
-include(EnableCXX11)
+if(UNIX)
+ include(EnableCXX11)
+endif()
include(MavrosMavlink)
# detect if sensor_msgs has BatteryState.msg
diff --git a/include/mavros/mavros_plugin.h b/include/mavros/mavros_plugin.h
index 8d8c2141b..99d739f50 100644
--- a/include/mavros/mavros_plugin.h
+++ b/include/mavros/mavros_plugin.h
@@ -66,6 +66,8 @@ public:
*/
virtual Subscriptions get_subscriptions() = 0;
+ UAS *m_uas;
+
protected:
/**
* @brief Plugin constructor
@@ -73,8 +75,6 @@ protected:
*/
PluginBase() : m_uas(nullptr) {};
- UAS *m_uas;
-
// TODO: filtered handlers
/**
diff --git a/include/mavros/setpoint_mixin.h b/include/mavros/setpoint_mixin.h
index 9156f1e66..0f8d545e7 100644
--- a/include/mavros/setpoint_mixin.h
+++ b/include/mavros/setpoint_mixin.h
@@ -181,7 +181,7 @@ public:
auto tf_transform_cb = std::bind(cbp, static_cast<D *>(this), std::placeholders::_1);
tf_thread = std::thread([this, tf_transform_cb]() {
- mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
+ //mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
@@ -217,7 +217,7 @@ public:
tf_thd_name = _thd_name;
tf_thread = std::thread([this, cbp, &topic_sub]() {
- mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
+ //mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
ros::NodeHandle &_sp_nh = static_cast<D *>(this)->sp_nh;
diff --git a/include/mavros/utils.h b/include/mavros/utils.h
index c670ce2c3..4992dd04f 100644
--- a/include/mavros/utils.h
+++ b/include/mavros/utils.h
@@ -24,6 +24,10 @@
#include <ros/console.h>
+#ifdef PASSTHROUGH
+#undef PASSTHROUGH
+#endif
+
// OS X compat: missing error codes
#ifdef __APPLE__
#define EBADE 50 /* Invalid exchange */
@@ -32,6 +36,14 @@
#define EBADSLT 55 /* Invalid slot */
#endif
+// Win compat: missing error codes
+#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#define EBADE 50 /* Invalid exchange */
+#define EBADFD 81 /* File descriptor in bad state */
+#define EBADRQC 54 /* Invalid request code */
+#define EBADSLT 55 /* Invalid slot */
+#endif
+
namespace mavros {
namespace utils {
using mavconn::utils::format;
diff --git a/src/lib/mavros.cpp b/src/lib/mavros.cpp
index 6a4ba43cf..bf837e7c6 100644
--- a/src/lib/mavros.cpp
+++ b/src/lib/mavros.cpp
@@ -14,7 +14,14 @@
#include <ros/console.h>
#include <mavros/mavros.h>
#include <mavros/utils.h>
+#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#include "windows.h"
+#include "shlwapi.h"
+#pragma comment(lib, "shlwapi.lib")
+#else
+#include <dirent.h>
#include <fnmatch.h>
+#endif
// MAVLINK_VERSION string
#include <mavlink/config.h>
@@ -263,6 +270,18 @@ void MavRos::plugin_route_cb(const mavlink_message_t *mmsg, const Framing framin
static bool pattern_match(std::string &pattern, std::string &pl_name)
{
+#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+ wchar_t wname[1024];
+ wchar_t wmask[1024];
+
+ size_t outsize;
+ mbstowcs_s(&outsize, wname, pattern.c_str(), strlen(pattern.c_str()) + 1);
+ mbstowcs_s(&outsize, wmask, pl_name.c_str(), strlen(pl_name.c_str()) + 1);
+ if (PathMatchSpecW(wname, wmask))
+ return true;
+ else
+ return false;
+#else
int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD);
if (cmp == 0)
return true;
@@ -274,6 +293,7 @@ static bool pattern_match(std::string &pattern, std::string &pl_name)
}
return false;
+#endif
}
/**
diff --git a/src/plugins/setpoint_attitude.cpp b/src/plugins/setpoint_attitude.cpp
index 9c9712aae..5196ade32 100644
--- a/src/plugins/setpoint_attitude.cpp
+++ b/src/plugins/setpoint_attitude.cpp
@@ -100,10 +100,15 @@ public:
return { /* Rx disabled */ };
}
+ ros::NodeHandle sp_nh;
+ std::string tf_frame_id;
+ std::string tf_child_frame_id;
+ double tf_rate;
+
private:
friend class SetAttitudeTargetMixin;
friend class TF2ListenerMixin;
- ros::NodeHandle sp_nh;
+
message_filters::Subscriber<mavros_msgs::Thrust> th_sub;
message_filters::Subscriber<geometry_msgs::PoseStamped> pose_sub;
@@ -112,11 +117,7 @@ private:
std::unique_ptr<SyncPoseThrust> sync_pose;
std::unique_ptr<SyncTwistThrust> sync_twist;
- std::string tf_frame_id;
- std::string tf_child_frame_id;
-
bool tf_listen;
- double tf_rate;
bool use_quaternion;
diff --git a/src/plugins/setpoint_position.cpp b/src/plugins/setpoint_position.cpp
index ebce9ec7b..3395e34d7 100644
--- a/src/plugins/setpoint_position.cpp
+++ b/src/plugins/setpoint_position.cpp
@@ -89,12 +89,16 @@ public:
return { /* Rx disabled */ };
}
+ ros::NodeHandle sp_nh;
+ std::string tf_frame_id;
+ std::string tf_child_frame_id;
+ double tf_rate;
+
private:
friend class SetPositionTargetLocalNEDMixin;
friend class SetPositionTargetGlobalIntMixin;
friend class TF2ListenerMixin;
- ros::NodeHandle sp_nh;
ros::NodeHandle spg_nh; //!< to get local position and gps coord which are not under sp_h()
ros::Subscriber setpoint_sub;
ros::Subscriber setpointg_sub; //!< Global setpoint
@@ -109,11 +113,7 @@ private:
Eigen::Vector3d current_local_pos; //!< Current local position in ENU
uint32_t old_gps_stamp = 0; //!< old time gps time stamp in [ms], to check if new gps msg is received
- std::string tf_frame_id;
- std::string tf_child_frame_id;
-
bool tf_listen;
- double tf_rate;
MAV_FRAME mav_frame;
diff --git a/src/plugins/sys_time.cpp b/src/plugins/sys_time.cpp
index 09103a5b5..78dad23dc 100644
--- a/src/plugins/sys_time.cpp
+++ b/src/plugins/sys_time.cpp
@@ -19,6 +19,9 @@
#include <sensor_msgs/TimeReference.h>
#include <std_msgs/Duration.h>
#include <mavros_msgs/TimesyncStatus.h>
+#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#include <time.h>
+#endif
namespace mavros {
namespace std_plugins {
@@ -477,7 +480,11 @@ private:
uint64_t get_monotonic_now(void)
{
struct timespec spec;
+#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+ timespec_get(&spec, TIME_UTC);
+#else
clock_gettime(CLOCK_MONOTONIC, &spec);
+#endif
return spec.tv_sec * 1000000000ULL + spec.tv_nsec;
}