diff --git a/drake_ros_examples/CMakeLists.txt b/drake_ros_examples/CMakeLists.txt
index ad2cadbf..89d1347e 100644
--- a/drake_ros_examples/CMakeLists.txt
+++ b/drake_ros_examples/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(gflags REQUIRED COMPONENTS shared)
add_subdirectory(examples/iiwa_manipulator)
add_subdirectory(examples/multirobot)
add_subdirectory(examples/rs_flip_flop)
+add_subdirectory(examples/ufo)
if(BUILD_TESTING)
find_package(ament_cmake_clang_format REQUIRED)
diff --git a/drake_ros_examples/README.md b/drake_ros_examples/README.md
index 02619ecc..b8241d6c 100644
--- a/drake_ros_examples/README.md
+++ b/drake_ros_examples/README.md
@@ -1,6 +1,7 @@
# Drake ROS Examples
-This is a collection of examples built around `drake_ros` libraries' C++ and Python APIs.
+This is a collection of examples built around `drake_ros` libraries' C++ and
+Python APIs.
## Building with Colcon
@@ -117,3 +118,5 @@ tooling per the list below.
- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm.
- [Multi-Robot Simulation](./examples/multirobot): Simulates multiple robots
using Drake and visualizes them with RViz using a single marker display topic.
+- [UFO](./examples/multirobot): an RViz visualization of a flying object
+controlled with the Drake systems framework and commanded using RViz.
diff --git a/drake_ros_examples/examples/ufo/CMakeLists.txt b/drake_ros_examples/examples/ufo/CMakeLists.txt
new file mode 100644
index 00000000..53aa6643
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/CMakeLists.txt
@@ -0,0 +1,27 @@
+find_package(ament_index_cpp REQUIRED)
+find_package(drake REQUIRED)
+find_package(drake_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+add_executable(ufo ufo.cc)
+target_link_libraries(ufo PRIVATE
+ ament_index_cpp::ament_index_cpp
+ drake::drake
+ drake_ros::drake_ros_core
+ drake_ros::drake_ros_tf2
+ drake_ros::drake_ros_viz
+ ${geometry_msgs_TARGETS}
+)
+target_compile_features(ufo PRIVATE cxx_std_17)
+
+install(
+ TARGETS
+ ufo
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+ DIRECTORY
+ models/
+ DESTINATION share/${PROJECT_NAME}/models/
+)
diff --git a/drake_ros_examples/examples/ufo/README.md b/drake_ros_examples/examples/ufo/README.md
new file mode 100644
index 00000000..a6fcf86c
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/README.md
@@ -0,0 +1,34 @@
+# UFO
+
+## Overview
+
+The UFO example shows how to use Drake ROS and the Drake systems framework to
+enable controlling a flying object with RViz.
+
+It publishes the following topics:
+
+* `/tf` (all scene frames)
+* `/scene_markers/collision` (all collision geometries)
+* `/scene_markers/visual` (all visual geometries)
+
+It subscribes to the following topic
+
+* `/goal_pose` (commands where the object should fly to)
+
+## How to run the example
+
+Run the `ufo` executable.
+
+```
+ros2 run drake_ros_examples ufo
+```
+
+Run RViz in a different terminal with your ROS installation sourced to visualize
+the station.
+
+```
+ros2 run rviz2 rviz2 -d ufo.rviz
+```
+
+Use the `2D Goal Pose` button in RViz to set the location to which the object
+should fly to.
diff --git a/drake_ros_examples/examples/ufo/models/ground.sdf b/drake_ros_examples/examples/ufo/models/ground.sdf
new file mode 100644
index 00000000..5359a2d5
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/models/ground.sdf
@@ -0,0 +1,25 @@
+
+
+
+ true
+
+
+
+
+ 50 50
+
+
+
+ 0.0 0.5 0.0 1.0
+
+
+
+
+
+ 50 50
+
+
+
+
+
+
diff --git a/drake_ros_examples/examples/ufo/models/ufo.sdf b/drake_ros_examples/examples/ufo/models/ufo.sdf
new file mode 100644
index 00000000..005925fe
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/models/ufo.sdf
@@ -0,0 +1,79 @@
+
+
+
+
+
+
+
+
+ 0 0 0.5 0 0 0
+
+
+
+ 2.5 2.5 0.5
+
+
+
+ 0.5 0.5 0.5 1.0
+
+
+
+
+
+ 2.5 2.5 0.5
+
+
+
+
+ 1000
+
+ 1300
+ 0
+ 0
+ 1300
+ 0
+ 2500
+
+
+
+
+
+
+ 0 0 0.49 0 0 0
+
+
+
+ 0.98
+
+
+
+ 1.0 0.55 0.0 0.35
+
+
+
+
+
+ 0.98
+
+
+
+
+ 100
+
+ 65.34
+ 0
+ 0
+ 65.34
+ 0
+ 65.34
+
+
+
+
+
+
+ spacecraft
+ lookout
+
+
+
diff --git a/drake_ros_examples/examples/ufo/models/ufo_scene.sdf b/drake_ros_examples/examples/ufo/models/ufo_scene.sdf
new file mode 100644
index 00000000..04104408
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/models/ufo_scene.sdf
@@ -0,0 +1,16 @@
+
+
+
+
+
+ package://drake_ros_examples/models/ufo.sdf
+
+
+
+
+ package://drake_ros_examples/models/ground.sdf
+
+ true
+
+
+
diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc
new file mode 100644
index 00000000..021d2f66
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/ufo.cc
@@ -0,0 +1,404 @@
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using drake::multibody::BodyIndex;
+using drake::multibody::ModelInstanceIndex;
+using drake::multibody::Parser;
+using drake::systems::AbstractStateIndex;
+using drake_ros::core::DrakeRos;
+using drake_ros::core::RosInterfaceSystem;
+using drake_ros::core::RosSubscriberSystem;
+using drake_ros::tf2::SceneTfBroadcasterSystem;
+using drake_ros::viz::RvizVisualizer;
+using Eigen::Quaterniond;
+using Eigen::Vector3d;
+
+using Bodyd = drake::multibody::Body;
+using Contextd = drake::systems::Context;
+using DiagramBuilderd = drake::systems::DiagramBuilder;
+using Diagramd = drake::systems::Diagram;
+using ExternallyAppliedSpatialForced =
+ drake::multibody::ExternallyAppliedSpatialForce;
+using LeafSystemd = drake::systems::LeafSystem;
+using MultibodyPlantd = drake::multibody::MultibodyPlant;
+using RigidTransformd = drake::math::RigidTransform;
+using Simulatord = drake::systems::Simulator;
+using SpatialForced = drake::multibody::SpatialForce;
+using Stated = drake::systems::State;
+
+/// Adds UFO scene to the multibody plant.
+/// \return index of flying saucer
+ModelInstanceIndex AddUfoScene(MultibodyPlantd* plant) {
+ auto parser = Parser(plant);
+ parser.package_map().Add(
+ "drake_ros_examples",
+ ament_index_cpp::get_package_share_directory("drake_ros_examples"));
+
+ // TODO(sloretz) make Drake Parser support package://
+ std::filesystem::path ufo_path{
+ parser.package_map().GetPath("drake_ros_examples")};
+ parser.AddAllModelsFromFile((ufo_path / "models/ufo_scene.sdf").string());
+
+ return plant->GetModelInstanceByName("spacecraft");
+}
+
+class FlyingSaucerController : public LeafSystemd {
+ public:
+ struct Params {
+ double saucer_mass;
+ Vector3d gravity;
+ Vector3d kp;
+ Vector3d kd;
+ Vector3d kp_rot;
+ Vector3d kd_rot;
+ double period{1.0 / 100.0};
+ };
+
+ explicit FlyingSaucerController(const Params& params)
+ : saucer_mass_(params.saucer_mass),
+ gravity_(params.gravity),
+ kp_(params.kp),
+ kd_(params.kd),
+ kp_rot_(params.kp_rot),
+ kd_rot_(params.kd_rot),
+ period_(params.period) {
+ DeclareAbstractInputPort(kCurrentPosePort,
+ *drake::AbstractValue::Make(RigidTransformd()));
+ DeclareAbstractInputPort(kTargetPosePort,
+ *drake::AbstractValue::Make(RigidTransformd()));
+
+ DeclareAbstractOutputPort(kSpatialForcePort,
+ &FlyingSaucerController::CalcSpatialForce);
+
+ X_WS_idx_ =
+ DeclareAbstractState(*drake::AbstractValue::Make(RigidTransformd()));
+ prev_X_WS_idx_ =
+ DeclareAbstractState(*drake::AbstractValue::Make(RigidTransformd()));
+
+ DeclarePeriodicUnrestrictedUpdateEvent(
+ period_, 0., &FlyingSaucerController::UpdateState);
+ }
+
+ // Pose of saucer in world frame
+ static constexpr const char* kCurrentPosePort{"X_WS"};
+ // Target saucer pose in world frame
+ static constexpr const char* kTargetPosePort{"X_WT"};
+ // SpatialForce to be applied on saucer in world frame
+ static constexpr const char* kSpatialForcePort{"F_S_W"};
+
+ private:
+ void UpdateState(const Contextd& context, Stated* state) const {
+ auto& current_pose_port = GetInputPort(kCurrentPosePort);
+
+ // Store current pose as old pose
+ state->get_mutable_abstract_state(prev_X_WS_idx_) =
+ state->get_mutable_abstract_state(X_WS_idx_);
+
+ // TODO(sloretz) what happens if UpdateState is called faster than
+ // X_WS input?
+ // Store input pose as current pose
+ state->get_mutable_abstract_state(X_WS_idx_) =
+ current_pose_port.Eval(context);
+ }
+
+ void CalcSpatialForce(const Contextd& context, SpatialForced* output) const {
+ // Get target pose
+ auto& target_pose_port = GetInputPort(kTargetPosePort);
+ const auto& X_WT = target_pose_port.Eval(context);
+
+ // Get Current and previous pose
+ const auto& avs = context.get_abstract_state();
+ const auto& X_WS = avs.get_value(X_WS_idx_).get_value();
+ const auto& prev_X_WS =
+ avs.get_value(prev_X_WS_idx_).get_value();
+
+ // force to be applied to saucer in world frame
+ Vector3d f_S_W{0, 0, 0};
+ // torque to be applied to saucer in world frame
+ Vector3d t_S_W{0, 0, 0};
+
+ // Translation
+ // p_WS = Current saucer position in world frame
+ // p_WT = Target saucer position in world frame
+ const auto& p_WS = X_WS.translation();
+ const auto& p_WT = X_WT.translation();
+
+ // Translation Error
+ const auto p_error = p_WT - p_WS;
+ const auto prev_p_error = p_WT - prev_X_WS.translation();
+
+ // Translation PD controller - proportional
+ f_S_W += ((p_error).array() * kp_.array()).matrix();
+
+ // Translation PD controller - derivative
+ const auto de_dt = (p_error - prev_p_error) / period_;
+ f_S_W += (de_dt.array() * kd_.array()).matrix();
+
+ // Translation PD Gravity feedforward
+ f_S_W -= saucer_mass_ * gravity_;
+
+ // Orientation
+ const auto& R_WS = X_WS.rotation();
+ const auto& R_WT = X_WT.rotation();
+
+ // Rotation from current to target orientation
+ const auto R_error = R_WT * R_WS.inverse();
+ const auto prev_R_error = R_WT * prev_X_WS.rotation().inverse();
+
+ // Orientation PD controller - proportional
+ t_S_W +=
+ (R_error.ToRollPitchYaw().vector().array() * kp_rot_.array()).matrix();
+
+ // Orientation PD controller - derivative
+ const auto rot_de_dt =
+ (R_error * prev_R_error.inverse()).ToRollPitchYaw().vector() / period_;
+ t_S_W += (rot_de_dt.array() * kd_rot_.array()).matrix();
+
+ *output = SpatialForced(t_S_W, f_S_W);
+ }
+
+ const double saucer_mass_;
+ const Vector3d gravity_;
+ const Vector3d kp_;
+ const Vector3d kd_;
+ const Vector3d kp_rot_;
+ const Vector3d kd_rot_;
+ const double period_;
+
+ AbstractStateIndex X_WS_idx_;
+ AbstractStateIndex prev_X_WS_idx_;
+};
+
+class BodyPoseAtIndex : public LeafSystemd {
+ public:
+ explicit BodyPoseAtIndex(BodyIndex index) : index_(index) {
+ DeclareAbstractInputPort(
+ kBodyPosesPort,
+ *drake::AbstractValue::Make(std::vector{}));
+
+ DeclareAbstractOutputPort(kPosePort, &BodyPoseAtIndex::CalcPose);
+ }
+
+ virtual ~BodyPoseAtIndex() = default;
+
+ static constexpr const char* kBodyPosesPort{"body_poses"};
+ static constexpr const char* kPosePort{"pose"};
+
+ private:
+ void CalcPose(const Contextd& context, RigidTransformd* output) const {
+ auto& body_poses_port = GetInputPort(kBodyPosesPort);
+
+ const auto& body_poses =
+ body_poses_port.Eval>(context);
+ *output = body_poses.at(index_);
+ }
+
+ const BodyIndex index_;
+};
+
+class AppliedSpatialForceVector : public LeafSystemd {
+ public:
+ explicit AppliedSpatialForceVector(BodyIndex index) : index_(index) {
+ DeclareAbstractInputPort(kSpatialForcePort,
+ *drake::AbstractValue::Make(SpatialForced()));
+
+ DeclareAbstractOutputPort(
+ kAppliedSpatialForcePort,
+ &AppliedSpatialForceVector::CalcAppliedSpatialForceVector);
+ }
+
+ virtual ~AppliedSpatialForceVector() = default;
+
+ static constexpr const char* kSpatialForcePort{"body_poses"};
+ static constexpr const char* kAppliedSpatialForcePort{
+ "applied_spatial_force"};
+
+ private:
+ void CalcAppliedSpatialForceVector(
+ const Contextd& context,
+ std::vector* output) const {
+ auto& input_port = GetInputPort(kSpatialForcePort);
+
+ const auto& F_WB = input_port.Eval(context);
+ ExternallyAppliedSpatialForced spatial_force;
+ spatial_force.body_index = index_;
+ spatial_force.p_BoBq_B = Vector3d{0, 0, 0};
+ spatial_force.F_Bq_W = F_WB;
+ output->clear();
+ output->push_back(spatial_force);
+ }
+
+ const BodyIndex index_;
+};
+
+class RigidTransformPremultiplier : public LeafSystemd {
+ public:
+ explicit RigidTransformPremultiplier(const RigidTransformd& X_BC)
+ : X_BC_(X_BC) {
+ DeclareAbstractInputPort(kInputPort,
+ *drake::AbstractValue::Make(RigidTransformd()));
+
+ DeclareAbstractOutputPort(kOutputPort,
+ &RigidTransformPremultiplier::CalcTransform);
+ }
+
+ static constexpr const char* kInputPort{"X_AB"};
+ static constexpr const char* kOutputPort{"X_AC"};
+
+ protected:
+ void CalcTransform(const Contextd& context, RigidTransformd* X_AC) const {
+ auto& input_port = GetInputPort(kInputPort);
+ auto& X_AB = input_port.Eval(context);
+ *X_AC = X_AB * X_BC_;
+ }
+
+ const RigidTransformd X_BC_;
+};
+
+class RosPoseGlue : public LeafSystemd {
+ public:
+ RosPoseGlue() {
+ DeclareAbstractInputPort(
+ kRosMsgPort,
+ *drake::AbstractValue::Make(geometry_msgs::msg::PoseStamped()));
+
+ DeclareAbstractOutputPort(kOutputPort, &RosPoseGlue::CalcDrakeTransform);
+ }
+
+ virtual ~RosPoseGlue() = default;
+
+ static constexpr const char* kRosMsgPort{"ros_pose"};
+ static constexpr const char* kOutputPort{"drake_pose"};
+
+ private:
+ void CalcDrakeTransform(const Contextd& context,
+ RigidTransformd* output) const {
+ auto& input_port = GetInputPort(kRosMsgPort);
+
+ // TODO(sloretz) use RobotLocomotion/drake-ros#141
+ const auto& goal_pose =
+ input_port.Eval(context);
+
+ *output = drake_ros::core::RosPoseToRigidTransform(goal_pose.pose);
+ }
+};
+
+/// Build a simulation and set initial conditions.
+std::unique_ptr BuildSimulation() {
+ DiagramBuilderd builder;
+
+ auto [plant, scene_graph] =
+ drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.001);
+
+ ModelInstanceIndex saucer_idx = AddUfoScene(&plant);
+
+ plant.Finalize();
+
+ double saucer_mass{0.0};
+ auto body_idxs = plant.GetBodyIndices(saucer_idx);
+ for (auto& body_idx : body_idxs) {
+ const Bodyd& body = plant.get_body(body_idx);
+ saucer_mass += body.default_mass();
+ }
+
+ FlyingSaucerController::Params params;
+ params.saucer_mass = saucer_mass;
+ params.gravity = plant.gravity_field().gravity_vector();
+ params.kp =
+ Vector3d{100.0, 100.0, 100.0}; // kp & kd chosen by trial and error
+ params.kd = Vector3d{500.0, 500.0, 400.0};
+ params.kp_rot =
+ Vector3d{1000.0, 1000.0, 1000.0}; // kp_rot & kd_rot also trial and error
+ params.kd_rot = Vector3d{1000.0, 1000.0, 1000.0};
+ auto ufo_controller = builder.AddSystem(params);
+
+ // Glue controller to multibody plant
+ // Get saucer poses X_WS to controller
+ const BodyIndex ufo_index = plant.GetBodyByName("spacecraft").index();
+ auto* body_pose_at_index = builder.AddSystem(ufo_index);
+ builder.Connect(
+ plant.get_body_poses_output_port(),
+ body_pose_at_index->GetInputPort(BodyPoseAtIndex::kBodyPosesPort));
+ builder.Connect(body_pose_at_index->GetOutputPort(BodyPoseAtIndex::kPosePort),
+ ufo_controller->GetInputPort("X_WS"));
+
+ // Connect UFO Controller's output to applied force accepted by MultibodyPlant
+ auto* apply_force_glue =
+ builder.AddSystem(ufo_index);
+ builder.Connect(ufo_controller->GetOutputPort("F_S_W"),
+ apply_force_glue->get_input_port());
+ builder.Connect(apply_force_glue->get_output_port(),
+ plant.get_applied_spatial_force_input_port());
+
+ // Add basic system to create a ROS node
+ auto* ros_interface_system = builder.AddSystem(
+ std::make_unique("systems_framework_demo"));
+
+ // Add a TF2 broadcaster to provide frame info to ROS
+ auto* scene_tf_broadcaster = builder.AddSystem(
+ ros_interface_system->get_ros_interface());
+ builder.Connect(scene_graph.get_query_output_port(),
+ scene_tf_broadcaster->get_graph_query_input_port());
+
+ // Add a system to output the visualisation markers for RViz
+ auto* scene_visualizer = builder.AddSystem(
+ ros_interface_system->get_ros_interface());
+ scene_visualizer->RegisterMultibodyPlant(&plant);
+ builder.Connect(scene_graph.get_query_output_port(),
+ scene_visualizer->get_graph_query_input_port());
+
+ // Add system to get goal pose from RViz
+ auto* goal_sub = builder.AddSystem(
+ RosSubscriberSystem::Make(
+ "goal_pose", rclcpp::QoS(1),
+ ros_interface_system->get_ros_interface()));
+ auto goal_glue = builder.AddSystem();
+ builder.Connect(goal_sub->get_output_port(), goal_glue->get_input_port());
+
+ // RViz goal is on the ground - add system to raise it by 10 meters
+ auto goal_offsetter = builder.AddSystem(
+ RigidTransformd(Vector3d{0, 0, 10.0}));
+ builder.Connect(goal_glue->get_output_port(),
+ goal_offsetter->get_input_port());
+ builder.Connect(goal_offsetter->get_output_port(),
+ ufo_controller->GetInputPort("X_WT"));
+
+ return builder.Build();
+}
+
+int main() {
+ drake_ros::core::init();
+
+ std::unique_ptr diagram = BuildSimulation();
+ std::unique_ptr diagram_context = diagram->CreateDefaultContext();
+
+ auto simulator =
+ std::make_unique(*diagram, std::move(diagram_context));
+
+ Contextd& simulator_context = simulator->get_mutable_context();
+ simulator->get_mutable_integrator().set_maximum_step_size(1.0 / 50.0);
+ simulator->set_target_realtime_rate(1.0);
+
+ simulator->Initialize();
+
+ while (true) {
+ simulator->AdvanceTo(simulator_context.get_time() + 0.1);
+ }
+
+ return 0;
+}
diff --git a/drake_ros_examples/examples/ufo/ufo.rviz b/drake_ros_examples/examples/ufo/ufo.rviz
new file mode 100644
index 00000000..9cb3ae3f
--- /dev/null
+++ b/drake_ros_examples/examples/ufo/ufo.rviz
@@ -0,0 +1,172 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 138
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /TF1
+ - /TF1/Frames1
+ - /TF1/Tree1
+ Splitter Ratio: 0.5
+ Tree Height: 730
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 50
+ Reference Frame:
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MarkerArray
+ Namespaces:
+ plant: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /scene_markers/visual
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ DefaultModelInstance/FlyingSaucer/20:
+ Value: true
+ FlyingSaucer:
+ Value: true
+ world:
+ Value: true
+ Marker Scale: 10
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ world:
+ DefaultModelInstance/FlyingSaucer/20:
+ {}
+ FlyingSaucer:
+ {}
+ Update Interval: 0
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 54.73565673828125
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1207
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001dc000003c8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000003c80000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000003c8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000003c80000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007c300000051fc0100000002fb0000000800540069006d00650100000000000007c30000045300fffffffb0000000800540069006d0065010000000000000450000000000000000000000480000003c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1987
+ X: 144
+ Y: 60