Skip to content

Commit a4c0322

Browse files
Jaeyoung-LimJaeyoung-Lim
committed
Fix more planning panels
1 parent 5638b60 commit a4c0322

File tree

6 files changed

+97
-47
lines changed

6 files changed

+97
-47
lines changed

‎mav_planning_rviz/src/planning_panel.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -621,7 +621,7 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m
621621
set_planner_state_buttons_[0]->setDisabled(true); // NAVIGATE
622622
set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT
623623
set_planner_state_buttons_[2]->setDisabled(false); // ABORT
624-
set_planner_state_buttons_[3]->setDisabled(false); // RETURN
624+
set_planner_state_buttons_[3]->setDisabled(false); // RETURN
625625
break;
626626
}
627627
case PLANNER_STATE::ROLLOUT: {
@@ -639,10 +639,10 @@ void PlanningPanel::plannerstateCallback(const planner_msgs::NavigationStatus& m
639639
break;
640640
}
641641
case PLANNER_STATE::RETURN: {
642-
set_planner_state_buttons_[0]->setDisabled(true); // NAVIGATE
643-
set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT
644-
set_planner_state_buttons_[2]->setDisabled(false); // ABORT
645-
set_planner_state_buttons_[3]->setDisabled(true); // RETURN
642+
set_planner_state_buttons_[0]->setDisabled(true); // NAVIGATE
643+
set_planner_state_buttons_[1]->setDisabled(true); // ROLLOUT
644+
set_planner_state_buttons_[2]->setDisabled(false); // ABORT
645+
set_planner_state_buttons_[3]->setDisabled(true); // RETURN
646646
break;
647647
}
648648
}

‎terrain_navigation/include/terrain_navigation/viewpoint.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -143,11 +143,11 @@ class ViewPoint {
143143
double getPixelResolution() { return 0.5 * M_PI / image_width_; }
144144

145145
/**
146-
* @brief
147-
*
148-
* @param pos
149-
* @return true
150-
* @return false
146+
* @brief
147+
*
148+
* @param pos
149+
* @return true
150+
* @return false
151151
*/
152152
bool isInsideViewFustrum(const Eigen::Vector3d &pos) {
153153
bool is_inside{false};
@@ -156,9 +156,9 @@ class ViewPoint {
156156
/// TODO: Get camera intrinsics from a file
157157
double f = image_width_ / 2;
158158
int c1 = f * camera_frame_pos(0) / camera_frame_pos(2);
159-
int c2 = f * camera_frame_pos(1) / camera_frame_pos(2);
159+
int c2 = f * camera_frame_pos(1) / camera_frame_pos(2);
160160

161-
if (std::abs(c1) < image_width_/2 && std::abs(c2) < image_height_/2 && camera_frame_pos(2) > 0.0) {
161+
if (std::abs(c1) < image_width_ / 2 && std::abs(c2) < image_height_ / 2 && camera_frame_pos(2) > 0.0) {
162162
is_inside = true;
163163
}
164164
return is_inside;

‎terrain_planner/include/terrain_planner/maneuver_library.h‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,9 +51,11 @@ class ManeuverLibrary {
5151
public:
5252
ManeuverLibrary();
5353
virtual ~ManeuverLibrary();
54-
std::shared_ptr<Primitive>& generateMotionPrimitives(const Eigen::Vector3d current_pos, const Eigen::Vector3d current_vel,
55-
const Eigen::Vector4d current_att, Path& current_path,
56-
bool add_emergency = true, int tree_depth = 2, double planning_horizon = 10.0);
54+
std::shared_ptr<Primitive>& generateMotionPrimitives(const Eigen::Vector3d current_pos,
55+
const Eigen::Vector3d current_vel,
56+
const Eigen::Vector4d current_att, Path& current_path,
57+
bool add_emergency = true, int tree_depth = 2,
58+
double planning_horizon = 10.0);
5759
std::vector<Path>& getMotionPrimitives() { return motion_primitives_; }
5860
std::vector<Path>& getValidPrimitives() { return valid_primitives_; }
5961
Path getRandomPrimitive();

‎terrain_planner/include/terrain_planner/terrain_ompl_rrt.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,8 @@ class TerrainOmplRrt {
6060
* @param start_vel velocity of the start state
6161
* @param goal center of the goal loiter position
6262
*/
63-
void setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& start_vel, const Eigen::Vector3d& goal, double goal_radius = -1);
63+
void setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& start_vel, const Eigen::Vector3d& goal,
64+
double goal_radius = -1);
6465

6566
/**
6667
* @brief Setup problem with position, velocity of the start and goal state

‎terrain_planner/launch/config.rviz‎

Lines changed: 73 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,13 @@ Panels:
55
Property Tree Widget:
66
Expanded:
77
- /Grid1
8+
- /GridMap1
89
- /MarkerArray3
910
- /MarkerArray4
1011
- /Marker5
12+
- /GridMap4
1113
Splitter Ratio: 0.5
12-
Tree Height: 596
14+
Tree Height: 531
1315
- Class: rviz/Selection
1416
Name: Selection
1517
- Class: rviz/Tool Properties
@@ -25,7 +27,6 @@ Panels:
2527
Name: Views
2628
Splitter Ratio: 0.5
2729
- Class: rviz/Time
28-
Experimental: false
2930
Name: Time
3031
SyncMode: 0
3132
SyncSource: ""
@@ -34,11 +35,10 @@ Panels:
3435
Topic: ""
3536
- Class: mav_planning_rviz/PlanningPanel
3637
Name: PlanningPanel
37-
goal_altitude: ""
3838
namespace: ""
3939
odometry_topic: ""
4040
planner_name: sertig
41-
planning_budget: 10
41+
planning_budget: 4
4242
Preferences:
4343
PromptSaveOnExit: true
4444
Toolbars:
@@ -81,6 +81,7 @@ Visualization Manager:
8181
Z: 0
8282
Pose Color: 255; 85; 255
8383
Pose Style: None
84+
Queue Size: 10
8485
Radius: 0.029999999329447746
8586
Shaft Diameter: 0.10000000149011612
8687
Shaft Length: 0.10000000149011612
@@ -104,23 +105,27 @@ Visualization Manager:
104105
Z: 0
105106
Pose Color: 255; 85; 255
106107
Pose Style: None
108+
Queue Size: 10
107109
Radius: 0.029999999329447746
108110
Shaft Diameter: 0.10000000149011612
109111
Shaft Length: 0.10000000149011612
110112
Topic: /geometric_controller/path
111113
Unreliable: false
112114
Value: true
113-
- Alpha: 1
115+
- Alpha: 0.8999999761581421
114116
Autocompute Intensity Bounds: true
115117
Class: grid_map_rviz_plugin/GridMap
116118
Color: 200; 200; 200
117119
Color Layer: color
118120
Color Transformer: ColorLayer
121+
ColorMap: default
119122
Enabled: true
123+
Grid Cell Decimation: 1
124+
Grid Line Thickness: 0.10000000149011612
120125
Height Layer: elevation
121126
Height Transformer: GridMapLayer
122127
History Length: 1
123-
Invert Rainbow: false
128+
Invert ColorMap: false
124129
Max Color: 255; 255; 255
125130
Max Intensity: 10
126131
Min Color: 0; 0; 0
@@ -129,19 +134,22 @@ Visualization Manager:
129134
Show Grid Lines: false
130135
Topic: /grid_map
131136
Unreliable: false
132-
Use Rainbow: true
137+
Use ColorMap: true
133138
Value: true
134139
- Alpha: 0.5
135140
Autocompute Intensity Bounds: true
136141
Class: grid_map_rviz_plugin/GridMap
137142
Color: 85; 170; 255
138143
Color Layer: elevation
139144
Color Transformer: FlatColor
145+
ColorMap: default
140146
Enabled: true
147+
Grid Cell Decimation: 1
148+
Grid Line Thickness: 0.10000000149011612
141149
Height Layer: safety
142150
Height Transformer: GridMapLayer
143151
History Length: 1
144-
Invert Rainbow: false
152+
Invert ColorMap: false
145153
Max Color: 255; 255; 255
146154
Max Intensity: 10
147155
Min Color: 0; 0; 0
@@ -150,19 +158,22 @@ Visualization Manager:
150158
Show Grid Lines: false
151159
Topic: /grid_map
152160
Unreliable: false
153-
Use Rainbow: true
161+
Use ColorMap: true
154162
Value: true
155163
- Alpha: 0.20000000298023224
156164
Autocompute Intensity Bounds: true
157165
Class: grid_map_rviz_plugin/GridMap
158166
Color: 255; 0; 0
159167
Color Layer: elevation
160168
Color Transformer: FlatColor
169+
ColorMap: default
161170
Enabled: false
171+
Grid Cell Decimation: 1
172+
Grid Line Thickness: 0.10000000149011612
162173
Height Layer: max_elevation
163174
Height Transformer: Layer
164175
History Length: 1
165-
Invert Rainbow: false
176+
Invert ColorMap: false
166177
Max Color: 255; 255; 255
167178
Max Intensity: 10
168179
Min Color: 0; 0; 0
@@ -171,14 +182,16 @@ Visualization Manager:
171182
Show Grid Lines: false
172183
Topic: /grid_map
173184
Unreliable: false
174-
Use Rainbow: true
185+
Use ColorMap: true
175186
Value: false
176-
- Class: rviz/Axes
187+
- Alpha: 1
188+
Class: rviz/Axes
177189
Enabled: false
178190
Length: 100
179191
Name: Axes
180192
Radius: 10
181193
Reference Frame: map
194+
Show Trail: false
182195
Value: false
183196
- Class: rviz/MarkerArray
184197
Enabled: true
@@ -193,7 +206,7 @@ Visualization Manager:
193206
Marker Topic: /position_target
194207
Name: Marker
195208
Namespaces:
196-
reference: true
209+
{}
197210
Queue Size: 100
198211
Value: true
199212
- Class: rviz/Marker
@@ -246,6 +259,7 @@ Visualization Manager:
246259
Z: 0
247260
Pose Color: 255; 85; 255
248261
Pose Style: None
262+
Queue Size: 10
249263
Radius: 0.029999999329447746
250264
Shaft Diameter: 0.10000000149011612
251265
Shaft Length: 0.10000000149011612
@@ -273,23 +287,23 @@ Visualization Manager:
273287
Marker Topic: /path_segments
274288
Name: MarkerArray
275289
Namespaces:
276-
normals: true
290+
{}
277291
Queue Size: 100
278292
Value: true
279293
- Class: rviz/Marker
280294
Enabled: true
281295
Marker Topic: /candidate_start_marker
282296
Name: Marker
283297
Namespaces:
284-
start: true
298+
{}
285299
Queue Size: 100
286300
Value: true
287301
- Class: rviz/Marker
288302
Enabled: true
289303
Marker Topic: /candidate_goal_marker
290304
Name: Marker
291305
Namespaces:
292-
goal: true
306+
{}
293307
Queue Size: 100
294308
Value: true
295309
- Class: rviz/Marker
@@ -300,6 +314,38 @@ Visualization Manager:
300314
normals: true
301315
Queue Size: 100
302316
Value: true
317+
- Alpha: 0.8999999761581421
318+
Autocompute Intensity Bounds: true
319+
Class: grid_map_rviz_plugin/GridMap
320+
Color: 200; 200; 200
321+
Color Layer: elevation
322+
Color Transformer: GridMapLayer
323+
ColorMap: default
324+
Enabled: true
325+
Grid Cell Decimation: 1
326+
Grid Line Thickness: 0.10000000149011612
327+
Height Layer: elevation
328+
Height Transformer: GridMapLayer
329+
History Length: 1
330+
Invert ColorMap: false
331+
Max Color: 255; 255; 255
332+
Max Intensity: 10
333+
Min Color: 0; 0; 0
334+
Min Intensity: 0
335+
Name: GridMap
336+
Show Grid Lines: false
337+
Topic: /avalanche_map
338+
Unreliable: false
339+
Use ColorMap: true
340+
Value: true
341+
- Class: rviz/MarkerArray
342+
Enabled: true
343+
Marker Topic: /planned_viewpoints
344+
Name: MarkerArray
345+
Namespaces:
346+
{}
347+
Queue Size: 100
348+
Value: true
303349
Enabled: true
304350
Global Options:
305351
Background Color: 255; 255; 255
@@ -328,35 +374,35 @@ Visualization Manager:
328374
Views:
329375
Current:
330376
Class: rviz/Orbit
331-
Distance: 2644.888427734375
377+
Distance: 3920.333984375
332378
Enable Stereo Rendering:
333379
Stereo Eye Separation: 0.05999999865889549
334380
Stereo Focal Distance: 1
335381
Swap Stereo Eyes: false
336382
Value: false
383+
Field of View: 0.7853981852531433
337384
Focal Point:
338-
X: -131.66195678710938
339-
Y: -378.06744384765625
340-
Z: 1392.7374267578125
385+
X: 55.76494216918945
386+
Y: 2079.845703125
387+
Z: 315.5940856933594
341388
Focal Shape Fixed Size: true
342389
Focal Shape Size: 0.05000000074505806
343390
Invert Z Axis: false
344391
Name: Current View
345392
Near Clip Distance: 0.009999999776482582
346-
Pitch: 0.6147981882095337
393+
Pitch: 0.49479860067367554
347394
Target Frame: map
348-
Value: Orbit (rviz)
349-
Yaw: 4.02869987487793
395+
Yaw: 4.263692378997803
350396
Saved: ~
351397
Window Geometry:
352398
Displays:
353-
collapsed: true
354-
Height: 1172
355-
Hide Left Dock: true
399+
collapsed: false
400+
Height: 1163
401+
Hide Left Dock: false
356402
Hide Right Dock: true
357403
PlanningPanel:
358404
collapsed: false
359-
QMainWindow State: 000000ff00000000fd0000000400000000000001a1000002dffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000007c000002df000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000004700ffffff000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000780000000fbfc0100000002fb0000001a0050006c0061006e006e0069006e006700500061006e0065006c0100000000000007800000027200fffffffb0000000800540069006d0065010000000000000450000000000000000000000780000002fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
405+
QMainWindow State: 000000ff00000000fd000000040000000000000247000003f6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000007a0000029c000000c700fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c010000031c000001540000014e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000004500ffffff000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000078000000039fc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d0065010000000000000450000000000000000000000533000003f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
360406
Selection:
361407
collapsed: false
362408
Teleop:

‎terrain_planner/src/maneuver_library.cpp‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -57,10 +57,11 @@ ManeuverLibrary::ManeuverLibrary() {
5757

5858
ManeuverLibrary::~ManeuverLibrary() {}
5959

60-
std::shared_ptr<Primitive>& ManeuverLibrary::generateMotionPrimitives(const Eigen::Vector3d current_pos,
61-
const Eigen::Vector3d current_vel,
62-
const Eigen::Vector4d current_att, Path &current_path,
63-
bool add_emergency, int tree_depth, double planning_horizon) {
60+
std::shared_ptr<Primitive> &ManeuverLibrary::generateMotionPrimitives(const Eigen::Vector3d current_pos,
61+
const Eigen::Vector3d current_vel,
62+
const Eigen::Vector4d current_att,
63+
Path &current_path, bool add_emergency,
64+
int tree_depth, double planning_horizon) {
6465
PathSegment current_segment;
6566
if (!current_path.segments.empty()) {
6667
current_segment = current_path.getCurrentSegment(current_pos);

0 commit comments

Comments
 (0)