1 |
commit: c1d3b78f6fd826f60503603d15c92c289cdd799c |
2 |
Author: Alexis Ballier <aballier <AT> gentoo <DOT> org> |
3 |
AuthorDate: Thu Jan 28 20:25:34 2016 +0000 |
4 |
Commit: Alexis Ballier <aballier <AT> gentoo <DOT> org> |
5 |
CommitDate: Thu Jan 28 20:25:43 2016 +0000 |
6 |
URL: https://gitweb.gentoo.org/repo/gentoo.git/commit/?id=c1d3b78f |
7 |
|
8 |
dev-ros/gazebo_plugins: backport upstream fixes to build with gazebo7 |
9 |
|
10 |
Package-Manager: portage-2.2.27 |
11 |
Signed-off-by: Alexis Ballier <aballier <AT> gentoo.org> |
12 |
|
13 |
dev-ros/gazebo_plugins/files/gazebo6.patch | 44 ++++++ |
14 |
dev-ros/gazebo_plugins/files/gazebo7-2.patch | 22 +++ |
15 |
dev-ros/gazebo_plugins/files/gazebo7-3.patch | 30 ++++ |
16 |
dev-ros/gazebo_plugins/files/gazebo7-4.patch | 21 +++ |
17 |
dev-ros/gazebo_plugins/files/gazebo7-5.patch | 146 +++++++++++++++++ |
18 |
dev-ros/gazebo_plugins/files/gazebo7.patch | 176 +++++++++++++++++++++ |
19 |
dev-ros/gazebo_plugins/gazebo_plugins-2.5.1.ebuild | 10 +- |
20 |
dev-ros/gazebo_plugins/gazebo_plugins-9999.ebuild | 2 +- |
21 |
8 files changed, 449 insertions(+), 2 deletions(-) |
22 |
|
23 |
diff --git a/dev-ros/gazebo_plugins/files/gazebo6.patch b/dev-ros/gazebo_plugins/files/gazebo6.patch |
24 |
new file mode 100644 |
25 |
index 0000000..43dd2d7 |
26 |
--- /dev/null |
27 |
+++ b/dev-ros/gazebo_plugins/files/gazebo6.patch |
28 |
@@ -0,0 +1,44 @@ |
29 |
+commit 04855dc0fd2d82d6fe72c0b54ae66a5f86c5ceb4 |
30 |
+Author: Steven Peters <scpeters@×××××××××××××.org> |
31 |
+Date: Tue Jan 12 17:36:06 2016 -0800 |
32 |
+ |
33 |
+ Fix gazebo6 deprecation warnings |
34 |
+ |
35 |
+ Several RaySensor functions are deprecated in gazebo6 |
36 |
+ and are removed in gazebo7. |
37 |
+ The return type is changed to use ignition math |
38 |
+ and the function name is changed. |
39 |
+ This adds ifdef's to handle the changes. |
40 |
+ |
41 |
+diff --git a/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/src/gazebo_ros_block_laser.cpp |
42 |
+index d8f40bc..76e0206 100644 |
43 |
+--- a/gazebo_plugins/src/gazebo_ros_block_laser.cpp |
44 |
++++ b/gazebo_plugins/src/gazebo_ros_block_laser.cpp |
45 |
+@@ -230,8 +230,13 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime) |
46 |
+ |
47 |
+ this->parent_ray_sensor_->SetActive(false); |
48 |
+ |
49 |
++#if GAZEBO_MAJOR_VERSION >= 6 |
50 |
++ auto maxAngle = this->parent_ray_sensor_->AngleMax(); |
51 |
++ auto minAngle = this->parent_ray_sensor_->AngleMin(); |
52 |
++#else |
53 |
+ math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax(); |
54 |
+ math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin(); |
55 |
++#endif |
56 |
+ |
57 |
+ double maxRange = this->parent_ray_sensor_->GetRangeMax(); |
58 |
+ double minRange = this->parent_ray_sensor_->GetRangeMin(); |
59 |
+@@ -240,8 +245,13 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime) |
60 |
+ |
61 |
+ int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount(); |
62 |
+ int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount(); |
63 |
++#if GAZEBO_MAJOR_VERSION >= 6 |
64 |
++ auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax(); |
65 |
++ auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin(); |
66 |
++#else |
67 |
+ math::Angle verticalMaxAngle = this->parent_ray_sensor_->GetVerticalAngleMax(); |
68 |
+ math::Angle verticalMinAngle = this->parent_ray_sensor_->GetVerticalAngleMin(); |
69 |
++#endif |
70 |
+ |
71 |
+ double yDiff = maxAngle.Radian() - minAngle.Radian(); |
72 |
+ double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian(); |
73 |
|
74 |
diff --git a/dev-ros/gazebo_plugins/files/gazebo7-2.patch b/dev-ros/gazebo_plugins/files/gazebo7-2.patch |
75 |
new file mode 100644 |
76 |
index 0000000..09af84e |
77 |
--- /dev/null |
78 |
+++ b/dev-ros/gazebo_plugins/files/gazebo7-2.patch |
79 |
@@ -0,0 +1,22 @@ |
80 |
+commit a27311b3b37a661aadb9815346d26e2970441bef |
81 |
+Author: Steven Peters <scpeters@×××××××××××××.org> |
82 |
+Date: Wed Jan 13 11:29:32 2016 -0800 |
83 |
+ |
84 |
+ Add missing boost header |
85 |
+ |
86 |
+ Some boost headers were remove from gazebo7 header files |
87 |
+ and gazebo_ros_joint_state_publisher.cpp was using it |
88 |
+ implicitly. |
89 |
+ |
90 |
+diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp |
91 |
+index 6c1ede1..d78b3d8 100644 |
92 |
+--- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp |
93 |
++++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp |
94 |
+@@ -25,6 +25,7 @@ |
95 |
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
96 |
+ * |
97 |
+ **/ |
98 |
++#include <boost/algorithm/string.hpp> |
99 |
+ #include <gazebo_plugins/gazebo_ros_joint_state_publisher.h> |
100 |
+ #include <tf/transform_broadcaster.h> |
101 |
+ #include <tf/transform_listener.h> |
102 |
|
103 |
diff --git a/dev-ros/gazebo_plugins/files/gazebo7-3.patch b/dev-ros/gazebo_plugins/files/gazebo7-3.patch |
104 |
new file mode 100644 |
105 |
index 0000000..895033d |
106 |
--- /dev/null |
107 |
+++ b/dev-ros/gazebo_plugins/files/gazebo7-3.patch |
108 |
@@ -0,0 +1,30 @@ |
109 |
+commit ce0ab101b272c4933f31945e2edaf215c4342772 |
110 |
+Author: Steven Peters <scpeters@×××××××××××××.org> |
111 |
+Date: Wed Jan 13 11:30:57 2016 -0800 |
112 |
+ |
113 |
+ Fix compiler error with SetHFOV |
114 |
+ |
115 |
+ In gazebo7, the rendering::Camera::SetHFOV function |
116 |
+ is overloaded with a potential for ambiguity, |
117 |
+ as reported in the following issue: |
118 |
+ https://bitbucket.org/osrf/gazebo/issues/1830 |
119 |
+ This fixes the build by explicitly defining the |
120 |
+ Angle type. |
121 |
+ |
122 |
+diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp |
123 |
+index 2129b65..4574e8d 100644 |
124 |
+--- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp |
125 |
++++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp |
126 |
+@@ -360,7 +360,11 @@ void GazeboRosCameraUtils::LoadThread() |
127 |
+ // Set Horizontal Field of View |
128 |
+ void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov) |
129 |
+ { |
130 |
+- this->camera_->SetHFOV(hfov->data); |
131 |
++#if GAZEBO_MAJOR_VERSION >= 7 |
132 |
++ this->camera_->SetHFOV(ignition::math::Angle(hfov->data)); |
133 |
++#else |
134 |
++ this->camera_->SetHFOV(gazebo::math::Angle(hfov->data)); |
135 |
++#endif |
136 |
+ } |
137 |
+ |
138 |
+ //////////////////////////////////////////////////////////////////////////////// |
139 |
|
140 |
diff --git a/dev-ros/gazebo_plugins/files/gazebo7-4.patch b/dev-ros/gazebo_plugins/files/gazebo7-4.patch |
141 |
new file mode 100644 |
142 |
index 0000000..0dab70d |
143 |
--- /dev/null |
144 |
+++ b/dev-ros/gazebo_plugins/files/gazebo7-4.patch |
145 |
@@ -0,0 +1,21 @@ |
146 |
+commit d7580215c9e39eddc33196b32b05219fcea707fd |
147 |
+Author: Steven Peters <scpeters@×××××××××××××.org> |
148 |
+Date: Fri Jan 15 16:29:37 2016 -0800 |
149 |
+ |
150 |
+ gazebo_ros_utils.h: include gazebo_config.h |
151 |
+ |
152 |
+ Make sure to include gazebo_config.h, |
153 |
+ which defines the GAZEBO_MAJOR_VERSION macro |
154 |
+ |
155 |
+diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h |
156 |
+index 6cdc4a8..6b016b8 100644 |
157 |
+--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h |
158 |
++++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h |
159 |
+@@ -39,6 +39,7 @@ |
160 |
+ #include <gazebo/common/common.hh> |
161 |
+ #include <gazebo/physics/physics.hh> |
162 |
+ #include <gazebo/sensors/Sensor.hh> |
163 |
++#include <gazebo/gazebo_config.h> |
164 |
+ #include <ros/ros.h> |
165 |
+ |
166 |
+ #ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST |
167 |
|
168 |
diff --git a/dev-ros/gazebo_plugins/files/gazebo7-5.patch b/dev-ros/gazebo_plugins/files/gazebo7-5.patch |
169 |
new file mode 100644 |
170 |
index 0000000..d83fdc2 |
171 |
--- /dev/null |
172 |
+++ b/dev-ros/gazebo_plugins/files/gazebo7-5.patch |
173 |
@@ -0,0 +1,146 @@ |
174 |
+commit 19f5f19f48def0a2c5808a7266151eb8e06d7b63 |
175 |
+Author: Steven Peters <scpeters@×××××××××××××.org> |
176 |
+Date: Tue Apr 14 18:10:36 2015 -0700 |
177 |
+ |
178 |
+ Use Joint::SetParam for joint velocity motors |
179 |
+ |
180 |
+ Before gazebo5, Joint::SetVelocity and SetMaxForce |
181 |
+ were used to set joint velocity motors. |
182 |
+ The API has changed in gazebo5, to use Joint::SetParam |
183 |
+ instead. |
184 |
+ The functionality is still available through the SetParam API. |
185 |
+ |
186 |
+diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp |
187 |
+index 004fb90..68265a8 100644 |
188 |
+--- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp |
189 |
++++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp |
190 |
+@@ -100,8 +100,8 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf |
191 |
+ joints_.resize ( 2 ); |
192 |
+ joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" ); |
193 |
+ joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" ); |
194 |
+- joints_[LEFT]->SetMaxForce ( 0, wheel_torque ); |
195 |
+- joints_[RIGHT]->SetMaxForce ( 0, wheel_torque ); |
196 |
++ joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque ); |
197 |
++ joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque ); |
198 |
+ |
199 |
+ |
200 |
+ |
201 |
+@@ -170,8 +170,8 @@ void GazeboRosDiffDrive::Reset() |
202 |
+ pose_encoder_.theta = 0; |
203 |
+ x_ = 0; |
204 |
+ rot_ = 0; |
205 |
+- joints_[LEFT]->SetMaxForce ( 0, wheel_torque ); |
206 |
+- joints_[RIGHT]->SetMaxForce ( 0, wheel_torque ); |
207 |
++ joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque ); |
208 |
++ joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque ); |
209 |
+ } |
210 |
+ |
211 |
+ void GazeboRosDiffDrive::publishWheelJointState() |
212 |
+@@ -214,15 +214,15 @@ void GazeboRosDiffDrive::publishWheelTF() |
213 |
+ void GazeboRosDiffDrive::UpdateChild() |
214 |
+ { |
215 |
+ |
216 |
+- /* force reset SetMaxForce since Joint::Reset reset MaxForce to zero at |
217 |
++ /* force reset SetParam("fmax") since Joint::Reset reset MaxForce to zero at |
218 |
+ https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331 |
219 |
+ (this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 ) |
220 |
+ and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset |
221 |
+ (this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e) |
222 |
+ */ |
223 |
+ for ( int i = 0; i < 2; i++ ) { |
224 |
+- if ( fabs(wheel_torque -joints_[i]->GetMaxForce ( 0 )) > 1e-6 ) { |
225 |
+- joints_[i]->SetMaxForce ( 0, wheel_torque ); |
226 |
++ if ( fabs(wheel_torque -joints_[i]->GetParam ( "fmax", 0 )) > 1e-6 ) { |
227 |
++ joints_[i]->SetParam ( "fmax", 0, wheel_torque ); |
228 |
+ } |
229 |
+ } |
230 |
+ |
231 |
+@@ -248,8 +248,8 @@ void GazeboRosDiffDrive::UpdateChild() |
232 |
+ ( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) || |
233 |
+ ( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) { |
234 |
+ //if max_accel == 0, or target speed is reached |
235 |
+- joints_[LEFT]->SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) ); |
236 |
+- joints_[RIGHT]->SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) ); |
237 |
++ joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) ); |
238 |
++ joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) ); |
239 |
+ } else { |
240 |
+ if ( wheel_speed_[LEFT]>=current_speed[LEFT] ) |
241 |
+ wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update ); |
242 |
+@@ -264,8 +264,8 @@ void GazeboRosDiffDrive::UpdateChild() |
243 |
+ // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); |
244 |
+ // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); |
245 |
+ |
246 |
+- joints_[LEFT]->SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) ); |
247 |
+- joints_[RIGHT]->SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) ); |
248 |
++ joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) ); |
249 |
++ joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) ); |
250 |
+ } |
251 |
+ last_update_time_+= common::Time ( update_period_ ); |
252 |
+ } |
253 |
+diff --git a/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp b/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp |
254 |
+index fdaf61f..a24aba5 100644 |
255 |
+--- a/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp |
256 |
++++ b/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp |
257 |
+@@ -256,10 +256,10 @@ namespace gazebo { |
258 |
+ gzthrow(error); |
259 |
+ } |
260 |
+ |
261 |
+- joints[LEFT_FRONT]->SetMaxForce(0, torque); |
262 |
+- joints[RIGHT_FRONT]->SetMaxForce(0, torque); |
263 |
+- joints[LEFT_REAR]->SetMaxForce(0, torque); |
264 |
+- joints[RIGHT_REAR]->SetMaxForce(0, torque); |
265 |
++ joints[LEFT_FRONT]->SetParam("fmax", 0, torque); |
266 |
++ joints[RIGHT_FRONT]->SetParam("fmax", 0, torque); |
267 |
++ joints[LEFT_REAR]->SetParam("fmax", 0, torque); |
268 |
++ joints[RIGHT_REAR]->SetParam("fmax", 0, torque); |
269 |
+ |
270 |
+ // Make sure the ROS node for Gazebo has already been initialized |
271 |
+ if (!ros::isInitialized()) |
272 |
+@@ -308,10 +308,10 @@ namespace gazebo { |
273 |
+ |
274 |
+ // Update robot in case new velocities have been requested |
275 |
+ getWheelVelocities(); |
276 |
+- joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0)); |
277 |
+- joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0)); |
278 |
+- joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0)); |
279 |
+- joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0)); |
280 |
++ joints[LEFT_FRONT]->SetParam("vel", 0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0)); |
281 |
++ joints[RIGHT_FRONT]->SetParam("vel", 0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0)); |
282 |
++ joints[LEFT_REAR]->SetParam("vel", 0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0)); |
283 |
++ joints[RIGHT_REAR]->SetParam("vel", 0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0)); |
284 |
+ |
285 |
+ last_update_time_+= common::Time(update_period_); |
286 |
+ |
287 |
+diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp |
288 |
+index 97c5ebb..0e83d2a 100644 |
289 |
+--- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp |
290 |
++++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp |
291 |
+@@ -108,8 +108,8 @@ void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _ |
292 |
+ odomOptions["world"] = WORLD; |
293 |
+ gazebo_ros_->getParameter<OdomSource> ( odom_source_, "odometrySource", odomOptions, WORLD ); |
294 |
+ |
295 |
+- joint_wheel_actuated_->SetMaxForce ( 0, wheel_torque_ ); |
296 |
+- joint_steering_->SetMaxForce ( 0, wheel_torque_ ); |
297 |
++ joint_wheel_actuated_->SetParam ( "fmax", 0, wheel_torque_ ); |
298 |
++ joint_steering_->SetParam ( "fmax", 0, wheel_torque_ ); |
299 |
+ |
300 |
+ |
301 |
+ // Initialize update rate stuff |
302 |
+@@ -235,7 +235,7 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe |
303 |
+ applied_speed = current_speed - wheel_deceleration_ * dt; |
304 |
+ } |
305 |
+ } |
306 |
+- joint_wheel_actuated_->SetVelocity ( 0, applied_speed ); |
307 |
++ joint_wheel_actuated_->SetParam ( "vel", 0, applied_speed ); |
308 |
+ |
309 |
+ double current_angle = joint_steering_->GetAngle ( 0 ).Radian(); |
310 |
+ if(target_angle > +M_PI / 2.0) target_angle = +M_PI / 2.0; |
311 |
+@@ -249,7 +249,7 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe |
312 |
+ } else { |
313 |
+ applied_steering_speed = -steering_speed_; |
314 |
+ } |
315 |
+- joint_steering_->SetVelocity ( 0, applied_steering_speed ); |
316 |
++ joint_steering_->SetParam ( "vel", 0, applied_steering_speed ); |
317 |
+ }else { |
318 |
+ #if GAZEBO_MAJOR_VERSION >= 4 |
319 |
+ joint_steering_->SetPosition ( 0, applied_angle ); |
320 |
|
321 |
diff --git a/dev-ros/gazebo_plugins/files/gazebo7.patch b/dev-ros/gazebo_plugins/files/gazebo7.patch |
322 |
new file mode 100644 |
323 |
index 0000000..7b10e18 |
324 |
--- /dev/null |
325 |
+++ b/dev-ros/gazebo_plugins/files/gazebo7.patch |
326 |
@@ -0,0 +1,176 @@ |
327 |
+commit 194ebf81f025c450555ec8cf3a98653bb1307c4c |
328 |
+Author: Steven Peters <scpeters@×××××××××××××.org> |
329 |
+Date: Wed Jan 13 11:25:52 2016 -0800 |
330 |
+ |
331 |
+ Fix gazebo7 build errors |
332 |
+ |
333 |
+ The SensorPtr types have changed from boost:: pointers |
334 |
+ to std:: pointers, |
335 |
+ which requires boost::dynamic_pointer_cast to change to |
336 |
+ std::dynamic_pointer_cast. |
337 |
+ A helper macro is added that adds a `using` statement |
338 |
+ corresponding to the correct type of dynamic_pointer_cast. |
339 |
+ This macro should be narrowly scoped to protect |
340 |
+ other code. |
341 |
+ |
342 |
+diff --git a/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h b/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h |
343 |
+index ff38ef6..b3092d0 100644 |
344 |
+--- a/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h |
345 |
++++ b/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h |
346 |
+@@ -43,7 +43,7 @@ namespace gazebo |
347 |
+ unsigned int _width, unsigned int _height, |
348 |
+ unsigned int _depth, const std::string &_format); |
349 |
+ |
350 |
+- protected: boost::shared_ptr<sensors::MultiCameraSensor> parentSensor; |
351 |
++ protected: sensors::MultiCameraSensorPtr parentSensor; |
352 |
+ |
353 |
+ protected: std::vector<unsigned int> width, height, depth; |
354 |
+ protected: std::vector<std::string> format; |
355 |
+diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h |
356 |
+index 3db4c45..6cdc4a8 100644 |
357 |
+--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h |
358 |
++++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h |
359 |
+@@ -41,6 +41,14 @@ |
360 |
+ #include <gazebo/sensors/Sensor.hh> |
361 |
+ #include <ros/ros.h> |
362 |
+ |
363 |
++#ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST |
364 |
++# if GAZEBO_MAJOR_VERSION >= 7 |
365 |
++#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using std::dynamic_pointer_cast |
366 |
++# else |
367 |
++#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using boost::dynamic_pointer_cast |
368 |
++# endif |
369 |
++#endif |
370 |
++ |
371 |
+ namespace gazebo |
372 |
+ { |
373 |
+ |
374 |
+diff --git a/gazebo_plugins/src/MultiCameraPlugin.cpp b/gazebo_plugins/src/MultiCameraPlugin.cpp |
375 |
+index 8001a22..11f663c 100644 |
376 |
+--- a/gazebo_plugins/src/MultiCameraPlugin.cpp |
377 |
++++ b/gazebo_plugins/src/MultiCameraPlugin.cpp |
378 |
+@@ -17,6 +17,7 @@ |
379 |
+ #include <gazebo/sensors/DepthCameraSensor.hh> |
380 |
+ #include <gazebo/sensors/CameraSensor.hh> |
381 |
+ #include <gazebo_plugins/MultiCameraPlugin.h> |
382 |
++#include <gazebo_plugins/gazebo_ros_utils.h> |
383 |
+ |
384 |
+ using namespace gazebo; |
385 |
+ GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin) |
386 |
+@@ -40,15 +41,16 @@ void MultiCameraPlugin::Load(sensors::SensorPtr _sensor, |
387 |
+ if (!_sensor) |
388 |
+ gzerr << "Invalid sensor pointer.\n"; |
389 |
+ |
390 |
++ GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; |
391 |
+ this->parentSensor = |
392 |
+- boost::dynamic_pointer_cast<sensors::MultiCameraSensor>(_sensor); |
393 |
++ dynamic_pointer_cast<sensors::MultiCameraSensor>(_sensor); |
394 |
+ |
395 |
+ if (!this->parentSensor) |
396 |
+ { |
397 |
+ gzerr << "MultiCameraPlugin requires a CameraSensor.\n"; |
398 |
+- if (boost::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor)) |
399 |
++ if (dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor)) |
400 |
+ gzmsg << "It is a depth camera sensor\n"; |
401 |
+- if (boost::dynamic_pointer_cast<sensors::CameraSensor>(_sensor)) |
402 |
++ if (dynamic_pointer_cast<sensors::CameraSensor>(_sensor)) |
403 |
+ gzmsg << "It is a camera sensor\n"; |
404 |
+ } |
405 |
+ |
406 |
+diff --git a/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/src/gazebo_ros_block_laser.cpp |
407 |
+index 76e0206..d03b9f1 100644 |
408 |
+--- a/gazebo_plugins/src/gazebo_ros_block_laser.cpp |
409 |
++++ b/gazebo_plugins/src/gazebo_ros_block_laser.cpp |
410 |
+@@ -24,6 +24,7 @@ |
411 |
+ #include <assert.h> |
412 |
+ |
413 |
+ #include <gazebo_plugins/gazebo_ros_block_laser.h> |
414 |
++#include <gazebo_plugins/gazebo_ros_utils.h> |
415 |
+ |
416 |
+ #include <gazebo/physics/World.hh> |
417 |
+ #include <gazebo/physics/HingeJoint.hh> |
418 |
+@@ -86,7 +87,8 @@ void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) |
419 |
+ this->node_ = transport::NodePtr(new transport::Node()); |
420 |
+ this->node_->Init(worldName); |
421 |
+ |
422 |
+- this->parent_ray_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_); |
423 |
++ GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; |
424 |
++ this->parent_ray_sensor_ = dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_); |
425 |
+ |
426 |
+ if (!this->parent_ray_sensor_) |
427 |
+ gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent"); |
428 |
+diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp |
429 |
+index 059f1d9..f8dbdd0 100644 |
430 |
+--- a/gazebo_plugins/src/gazebo_ros_bumper.cpp |
431 |
++++ b/gazebo_plugins/src/gazebo_ros_bumper.cpp |
432 |
+@@ -39,6 +39,7 @@ |
433 |
+ #include <tf/tf.h> |
434 |
+ |
435 |
+ #include <gazebo_plugins/gazebo_ros_bumper.h> |
436 |
++#include <gazebo_plugins/gazebo_ros_utils.h> |
437 |
+ |
438 |
+ namespace gazebo |
439 |
+ { |
440 |
+@@ -65,7 +66,8 @@ GazeboRosBumper::~GazeboRosBumper() |
441 |
+ // Load the controller |
442 |
+ void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) |
443 |
+ { |
444 |
+- this->parentSensor = boost::dynamic_pointer_cast<sensors::ContactSensor>(_parent); |
445 |
++ GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; |
446 |
++ this->parentSensor = dynamic_pointer_cast<sensors::ContactSensor>(_parent); |
447 |
+ if (!this->parentSensor) |
448 |
+ { |
449 |
+ ROS_ERROR("Contact sensor parent is not of type ContactSensor"); |
450 |
+diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp |
451 |
+index 811fc81..6b36c48 100644 |
452 |
+--- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp |
453 |
++++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp |
454 |
+@@ -75,8 +75,9 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) |
455 |
+ // save pointers |
456 |
+ this->sdf = _sdf; |
457 |
+ |
458 |
++ GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; |
459 |
+ this->parent_ray_sensor_ = |
460 |
+- boost::dynamic_pointer_cast<sensors::GpuRaySensor>(_parent); |
461 |
++ dynamic_pointer_cast<sensors::GpuRaySensor>(_parent); |
462 |
+ |
463 |
+ if (!this->parent_ray_sensor_) |
464 |
+ gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); |
465 |
+diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp |
466 |
+index 815c456..80e60a2 100644 |
467 |
+--- a/gazebo_plugins/src/gazebo_ros_laser.cpp |
468 |
++++ b/gazebo_plugins/src/gazebo_ros_laser.cpp |
469 |
+@@ -72,8 +72,9 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) |
470 |
+ // save pointers |
471 |
+ this->sdf = _sdf; |
472 |
+ |
473 |
++ GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; |
474 |
+ this->parent_ray_sensor_ = |
475 |
+- boost::dynamic_pointer_cast<sensors::RaySensor>(_parent); |
476 |
++ dynamic_pointer_cast<sensors::RaySensor>(_parent); |
477 |
+ |
478 |
+ if (!this->parent_ray_sensor_) |
479 |
+ gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); |
480 |
+diff --git a/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_plugins/src/gazebo_ros_range.cpp |
481 |
+index 9387dde..cb229fe 100644 |
482 |
+--- a/gazebo_plugins/src/gazebo_ros_range.cpp |
483 |
++++ b/gazebo_plugins/src/gazebo_ros_range.cpp |
484 |
+@@ -35,6 +35,7 @@ |
485 |
+ /** \author Jose Capriles, Bence Magyar. */ |
486 |
+ |
487 |
+ #include "gazebo_plugins/gazebo_ros_range.h" |
488 |
++#include "gazebo_plugins/gazebo_ros_utils.h" |
489 |
+ |
490 |
+ #include <algorithm> |
491 |
+ #include <string> |
492 |
+@@ -92,8 +93,9 @@ void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) |
493 |
+ |
494 |
+ this->last_update_time_ = common::Time(0); |
495 |
+ |
496 |
++ GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; |
497 |
+ this->parent_ray_sensor_ = |
498 |
+- boost::dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_); |
499 |
++ dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_); |
500 |
+ |
501 |
+ if (!this->parent_ray_sensor_) |
502 |
+ gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent"); |
503 |
|
504 |
diff --git a/dev-ros/gazebo_plugins/gazebo_plugins-2.5.1.ebuild b/dev-ros/gazebo_plugins/gazebo_plugins-2.5.1.ebuild |
505 |
index 45c19f2..5a965c4 100644 |
506 |
--- a/dev-ros/gazebo_plugins/gazebo_plugins-2.5.1.ebuild |
507 |
+++ b/dev-ros/gazebo_plugins/gazebo_plugins-2.5.1.ebuild |
508 |
@@ -42,7 +42,7 @@ RDEPEND=" |
509 |
dev-ros/camera_info_manager |
510 |
dev-ros/moveit_msgs[${CATKIN_MESSAGES_CXX_USEDEP}] |
511 |
dev-libs/libxml2 |
512 |
- sci-electronics/gazebo |
513 |
+ >=sci-electronics/gazebo-7:= |
514 |
dev-games/ogre |
515 |
sci-libs/pcl |
516 |
dev-libs/boost:= |
517 |
@@ -51,3 +51,11 @@ RDEPEND=" |
518 |
dev-ros/roslib[${PYTHON_USEDEP}] |
519 |
" |
520 |
DEPEND="${RDEPEND}" |
521 |
+PATCHES=( |
522 |
+ "${FILESDIR}/gazebo6.patch" |
523 |
+ "${FILESDIR}/gazebo7.patch" |
524 |
+ "${FILESDIR}/gazebo7-2.patch" |
525 |
+ "${FILESDIR}/gazebo7-3.patch" |
526 |
+ "${FILESDIR}/gazebo7-4.patch" |
527 |
+ "${FILESDIR}/gazebo7-5.patch" |
528 |
+) |
529 |
|
530 |
diff --git a/dev-ros/gazebo_plugins/gazebo_plugins-9999.ebuild b/dev-ros/gazebo_plugins/gazebo_plugins-9999.ebuild |
531 |
index 45c19f2..2c0abf6 100644 |
532 |
--- a/dev-ros/gazebo_plugins/gazebo_plugins-9999.ebuild |
533 |
+++ b/dev-ros/gazebo_plugins/gazebo_plugins-9999.ebuild |
534 |
@@ -42,7 +42,7 @@ RDEPEND=" |
535 |
dev-ros/camera_info_manager |
536 |
dev-ros/moveit_msgs[${CATKIN_MESSAGES_CXX_USEDEP}] |
537 |
dev-libs/libxml2 |
538 |
- sci-electronics/gazebo |
539 |
+ >=sci-electronics/gazebo-7:= |
540 |
dev-games/ogre |
541 |
sci-libs/pcl |
542 |
dev-libs/boost:= |