Skip to content

Commit 380c29d

Browse files
srmainwaringsrmainwaring
authored
ros2: add curvature target marker (#50)
- Add marker and publish to /curvature_target. - Correct curvature marker direction. - Add curvature marker to rviz config. - Fix code style. Signed-off-by: Rhys Mainwaring <[email protected]>
1 parent f521515 commit 380c29d

File tree

3 files changed

+55
-18
lines changed

3 files changed

+55
-18
lines changed

‎terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -130,6 +130,9 @@ class TerrainPlanner : public rclcpp::Node {
130130
const Eigen::Vector3d &velocity, const double curvature);
131131
void publishReferenceMarker(rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
132132
const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, const double curvature);
133+
void publishReferenceCurvatureMarker(rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
134+
const Eigen::Vector3d &position, const Eigen::Vector3d &velocity,
135+
const double curvature);
133136
void publishVelocityMarker(rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
134137
const Eigen::Vector3d &position, const Eigen::Vector3d &velocity);
135138
void publishPathSegments(rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr, Path &trajectory);
@@ -179,6 +182,7 @@ class TerrainPlanner : public rclcpp::Node {
179182
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr referencehistory_pub_;
180183
rclcpp::Publisher<mavros_msgs::msg::GlobalPositionTarget>::SharedPtr global_position_setpoint_pub_;
181184
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr position_target_pub_;
185+
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr curvature_target_pub_;
182186
rclcpp::Publisher<planner_msgs::msg::NavigationStatus>::SharedPtr planner_status_pub_;
183187
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr goal_pub_;
184188
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr rallypoint_pub_;

‎terrain_navigation_ros/src/terrain_planner.cpp

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,7 @@ TerrainPlanner::TerrainPlanner() : Node("terrain_planner") {
107107
posehistory_pub_ = this->create_publisher<nav_msgs::msg::Path>("geometric_controller/path", 10);
108108
referencehistory_pub_ = this->create_publisher<nav_msgs::msg::Path>("reference/path", 10);
109109
position_target_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("position_target", 1);
110+
curvature_target_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("curvature_target", 1);
110111
vehicle_velocity_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("vehicle_velocity", 1);
111112
goal_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("goal_marker", 1);
112113
rallypoint_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("rallypoints_marker", 1);
@@ -260,6 +261,7 @@ void TerrainPlanner::cmdloopCallback() {
260261
GeoConversions::reverse(lv03_reference_position(0), lv03_reference_position(1), lv03_reference_position(2),
261262
latitude, longitude, altitude);
262263
publishReferenceMarker(position_target_pub_, reference_position, reference_tangent, reference_curvature);
264+
publishReferenceCurvatureMarker(curvature_target_pub_, reference_position, reference_tangent, reference_curvature);
263265

264266
// Run additional altitude control
265267
double altitude_correction = K_z_ * (vehicle_position_(2) - reference_position(2));
@@ -821,6 +823,23 @@ void TerrainPlanner::publishReferenceMarker(rclcpp::Publisher<visualization_msgs
821823
pub->publish(marker);
822824
}
823825

826+
void TerrainPlanner::publishReferenceCurvatureMarker(rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub,
827+
const Eigen::Vector3d &position, const Eigen::Vector3d &velocity,
828+
const double curvature) {
829+
// do not zero the radius completely for zero curvature as this results in
830+
// a warning when displaying the marker in rviz.
831+
double radius = std::fabs(curvature) < 1.0E-4 ? 1.0E-6 : 1.0 / std::fabs(curvature);
832+
double direction = curvature > 0.0 ? 1.0 : -1.0;
833+
auto direction_vector = Eigen::Vector3d(0.0, 0.0, direction);
834+
auto projected_velocity = Eigen::Vector3d(velocity(0), velocity(1), 0.0);
835+
auto unit_tangent_vector = projected_velocity.normalized();
836+
Eigen::Vector3d curvature_vector = radius * direction_vector.cross(unit_tangent_vector);
837+
visualization_msgs::msg::Marker marker =
838+
vector2ArrowsMsg(position, curvature_vector, 0, Eigen::Vector3d(1.0, 0.0, 0.0), "reference_curvature");
839+
840+
pub->publish(marker);
841+
}
842+
824843
void TerrainPlanner::publishPathSegments(rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub,
825844
Path &trajectory) {
826845
visualization_msgs::msg::MarkerArray msg;

‎terrain_planner/launch/config.rviz

Lines changed: 32 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ Panels:
88
- /Vehicle1
99
- /Vehicle1/VehiclePoseMarker1/Namespaces1
1010
Splitter Ratio: 0.5
11-
Tree Height: 285
11+
Tree Height: 280
1212
- Class: rviz_common/Selection
1313
Name: Selection
1414
- Class: rviz_common/Tool Properties
@@ -33,6 +33,7 @@ Panels:
3333
Topic: ""
3434
- Class: mav_planning_rviz/PlanningPanel
3535
Name: PlanningPanel
36+
flight_stack: px4
3637
namespace: ""
3738
odometry_topic: ""
3839
planner_name: davosdorf
@@ -227,6 +228,19 @@ Visualization Manager:
227228
Reliability Policy: Reliable
228229
Value: /position_target
229230
Value: true
231+
- Class: rviz_default_plugins/Marker
232+
Enabled: true
233+
Name: CurvatureTargetMarker
234+
Namespaces:
235+
{}
236+
Topic:
237+
Depth: 5
238+
Durability Policy: Volatile
239+
Filter size: 10
240+
History Policy: Keep Last
241+
Reliability Policy: Reliable
242+
Value: /curvature_target
243+
Value: true
230244
- Class: rviz_default_plugins/InteractiveMarkers
231245
Enable Transparency: true
232246
Enabled: true
@@ -287,14 +301,14 @@ Visualization Manager:
287301
- Alpha: 1
288302
Buffer Length: 1
289303
Class: rviz_default_plugins/Path
290-
Color: 0; 0; 255
304+
Color: 255; 255; 0
291305
Enabled: true
292306
Head Diameter: 0.30000001192092896
293307
Head Length: 0.20000000298023224
294308
Length: 0.30000001192092896
295309
Line Style: Lines
296310
Line Width: 0.029999999329447746
297-
Name: GeometricControllerPath
311+
Name: ReferencePath
298312
Offset:
299313
X: 0
300314
Y: 0
@@ -310,19 +324,19 @@ Visualization Manager:
310324
Filter size: 10
311325
History Policy: Keep Last
312326
Reliability Policy: Reliable
313-
Value: /geometric_controller/path
327+
Value: /reference/path
314328
Value: true
315329
- Alpha: 1
316330
Buffer Length: 1
317331
Class: rviz_default_plugins/Path
318-
Color: 255; 255; 0
332+
Color: 0; 255; 255
319333
Enabled: true
320334
Head Diameter: 0.30000001192092896
321335
Head Length: 0.20000000298023224
322336
Length: 0.30000001192092896
323337
Line Style: Lines
324338
Line Width: 0.029999999329447746
325-
Name: ReferencePath
339+
Name: PlannedVehiclePath
326340
Offset:
327341
X: 0
328342
Y: 0
@@ -338,7 +352,7 @@ Visualization Manager:
338352
Filter size: 10
339353
History Policy: Keep Last
340354
Reliability Policy: Reliable
341-
Value: /reference/path
355+
Value: /vehicle_path
342356
Value: true
343357
Enabled: true
344358
Name: Planning
@@ -385,14 +399,14 @@ Visualization Manager:
385399
- Alpha: 1
386400
Buffer Length: 1
387401
Class: rviz_default_plugins/Path
388-
Color: 0; 255; 255
402+
Color: 0; 0; 255
389403
Enabled: true
390404
Head Diameter: 0.30000001192092896
391405
Head Length: 0.20000000298023224
392406
Length: 0.30000001192092896
393407
Line Style: Lines
394408
Line Width: 0.029999999329447746
395-
Name: VehiclePath
409+
Name: VehiclePoseHistory
396410
Offset:
397411
X: 0
398412
Y: 0
@@ -408,7 +422,7 @@ Visualization Manager:
408422
Filter size: 10
409423
History Policy: Keep Last
410424
Reliability Policy: Reliable
411-
Value: /vehicle_path
425+
Value: /geometric_controller/path
412426
Value: true
413427
Enabled: true
414428
Name: Vehicle
@@ -459,25 +473,25 @@ Visualization Manager:
459473
Views:
460474
Current:
461475
Class: rviz_default_plugins/Orbit
462-
Distance: 4404.001953125
476+
Distance: 3783.384765625
463477
Enable Stereo Rendering:
464478
Stereo Eye Separation: 0.05999999865889549
465479
Stereo Focal Distance: 1
466480
Swap Stereo Eyes: false
467481
Value: false
468482
Focal Point:
469-
X: -1531.7305908203125
470-
Y: -52.1274299621582
471-
Z: 1152.6519775390625
483+
X: 808.8604736328125
484+
Y: -174.71060180664062
485+
Z: 1149.7601318359375
472486
Focal Shape Fixed Size: true
473487
Focal Shape Size: 0.05000000074505806
474488
Invert Z Axis: false
475489
Name: Current View
476490
Near Clip Distance: 0.009999999776482582
477-
Pitch: 0.25479769706726074
491+
Pitch: 1.5697963237762451
478492
Target Frame: map
479493
Value: Orbit (rviz_default_plugins)
480-
Yaw: 6.203699588775635
494+
Yaw: -1.5700000524520874
481495
Saved: ~
482496
Window Geometry:
483497
Displays:
@@ -487,7 +501,7 @@ Window Geometry:
487501
Hide Right Dock: true
488502
PlanningPanel:
489503
collapsed: false
490-
QMainWindow State: 000000ff00000000fd0000000400000000000002ad000002dafc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000640000016c000000e300fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c01000001d10000016d0000016d00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000007a00ffffff000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000052b00000037fc0100000002fb0000000800540069006d006501000000000000052b0000023d00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d006501000000000000045000000000000000000000027d000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
504+
QMainWindow State: 000000ff00000000fd0000000400000000000002ad000002dafc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006400000167000000e300fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c01000001cc000001720000017200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000007a00ffffff000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000064000002da000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000052b00000037fc0100000002fb0000000800540069006d006501000000000000052b0000023d00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d006501000000000000045000000000000000000000027d000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
491505
Selection:
492506
collapsed: false
493507
Teleop:
@@ -500,4 +514,4 @@ Window Geometry:
500514
collapsed: true
501515
Width: 1323
502516
X: 0
503-
Y: 25
517+
Y: 38

0 commit comments

Comments
 (0)