From 5e7c25b523341716f109518e72e3b6dfcd5ff299 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 31 Oct 2024 12:59:42 -0500 Subject: [PATCH] Updates to support visualizing online planning example --- tesseract_msgs/msg/JointTrajectory.msg | 1 + tesseract_msgs/msg/Trajectory.msg | 2 + .../config/online_example.rviz | 590 ++---------------- tesseract_rosutils/src/plotting.cpp | 13 +- tesseract_rosutils/src/utils.cpp | 4 + .../joint_trajectory_monitor_properties.cpp | 48 +- 6 files changed, 96 insertions(+), 562 deletions(-) diff --git a/tesseract_msgs/msg/JointTrajectory.msg b/tesseract_msgs/msg/JointTrajectory.msg index 7be10d60b..9d4fdf0ab 100644 --- a/tesseract_msgs/msg/JointTrajectory.msg +++ b/tesseract_msgs/msg/JointTrajectory.msg @@ -1,2 +1,3 @@ tesseract_msgs/JointState[] states string description # A description of the trajectory +string uuid # Unique ID diff --git a/tesseract_msgs/msg/Trajectory.msg b/tesseract_msgs/msg/Trajectory.msg index 6b0026234..43967fc57 100644 --- a/tesseract_msgs/msg/Trajectory.msg +++ b/tesseract_msgs/msg/Trajectory.msg @@ -25,4 +25,6 @@ tesseract_msgs/StringDoublePair[] initial_state string instructions # If instructions are provided this will be ignored +string joint_trajectories_uuid +string joint_trajectories_description tesseract_msgs/JointTrajectory[] joint_trajectories diff --git a/tesseract_ros_examples/config/online_example.rviz b/tesseract_ros_examples/config/online_example.rviz index 1b0d7a11f..916494d27 100644 --- a/tesseract_ros_examples/config/online_example.rviz +++ b/tesseract_ros_examples/config/online_example.rviz @@ -5,11 +5,11 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /TesseractTrajectory1/Trajectory Monitor1 - - /EnvironmentState1/Environment1 - - /EnvironmentState1/Links1/target_z1 - Splitter Ratio: 0.5077950954437256 - Tree Height: 746 + - /TesseractWorkbench1 + - /TesseractWorkbench1/Environment Properties1 + - /TesseractWorkbench1/Joint Trajectory Properties1 + Splitter Ratio: 0.49250534176826477 + Tree Height: 751 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -25,7 +25,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" @@ -54,549 +53,38 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: tesseract_rviz/TesseractTrajectory + - Class: rviz/MarkerArray Enabled: true - Environment: - Alpha: 1 - Display Mode: URDF - Show All Links: true - Show Collision: false - Show Highlights: true - Show Visual: true - URDF Parameter: robot_description - Value: "" - Joint State Monitor: - Topic: /joint_states - Value: "" - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Collision: - ACM: - floor: Never - gantry_axis_1: Never - gantry_axis_2: Adjacent - gantry_base: Never - human_estop_zone: Never - link_1: Adjacent - link_2: Never - link_3: Never - robot_estop_zone: Adjacent - Show Axes: false - Show Trail: false - Value: true - floor: - Alpha: 1 - Collision: - ACM: - base_link: Never - gantry_axis_1: Never - gantry_axis_2: Never - gantry_base: Adjacent - human_estop_zone: Adjacent - link_1: Never - link_2: Never - link_3: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - gantry_axis_1: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Never - gantry_axis_2: Adjacent - gantry_base: Adjacent - human_estop_zone: Never - link_1: Never - link_2: Never - link_3: Never - robot_estop_zone: Never - Show Axes: false - Show Trail: false - Value: true - gantry_axis_2: - Alpha: 1 - Collision: - ACM: - base_link: Adjacent - floor: Never - gantry_axis_1: Adjacent - gantry_base: Never - human_estop_zone: Never - link_1: Never - link_2: Never - link_3: Never - robot_estop_zone: Adjacent - Show Axes: false - Show Trail: false - Value: true - gantry_base: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Adjacent - gantry_axis_1: Adjacent - gantry_axis_2: Never - human_estop_zone: Default - link_1: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - gantry_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - human_estop_zone: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Adjacent - gantry_axis_1: Never - gantry_axis_2: Never - gantry_base: Default - link_1: Never - link_2: Default - link_3: Default - link_4: Default - link_5: Default - link_6: Default - Show Axes: false - Show Trail: false - Value: true - human_x: - Alpha: 1 - Show Axes: false - Show Trail: false - human_y: - Alpha: 1 - Show Axes: false - Show Trail: false - link_1: - Alpha: 1 - Collision: - ACM: - base_link: Adjacent - floor: Never - gantry_axis_1: Never - gantry_axis_2: Never - gantry_base: Never - human_estop_zone: Never - link_2: Adjacent - link_3: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_2: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Never - gantry_axis_1: Never - gantry_axis_2: Never - human_estop_zone: Default - link_1: Adjacent - link_3: Adjacent - link_4: Never - link_5: Never - link_6: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_3: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Never - gantry_axis_1: Never - gantry_axis_2: Never - human_estop_zone: Default - link_1: Never - link_2: Adjacent - link_4: Adjacent - link_5: Never - link_6: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_4: - Alpha: 1 - Collision: - ACM: - human_estop_zone: Default - link_2: Never - link_3: Adjacent - link_5: Adjacent - link_6: Default - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_5: - Alpha: 1 - Collision: - ACM: - human_estop_zone: Default - link_2: Never - link_3: Never - link_4: Adjacent - link_6: Adjacent - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_6: - Alpha: 1 - Collision: - ACM: - human_estop_zone: Default - link_2: Never - link_3: Never - link_4: Default - link_5: Adjacent - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - robot_estop_zone: - Alpha: 1 - Collision: - ACM: - base_link: Adjacent - floor: Default - gantry_axis_1: Never - gantry_axis_2: Adjacent - gantry_base: Default - link_1: Default - link_2: Default - link_3: Default - link_4: Default - link_5: Default - link_6: Default - Show Axes: false - Show Trail: false - Value: true - target_x: - Alpha: 1 - Show Axes: false - Show Trail: false - target_y: - Alpha: 1 - Show Axes: false - Show Trail: false - target_z: - Alpha: 1 - Show Axes: false - Show Trail: false - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: TesseractTrajectory - Trajectory Monitor: - Topic: /tesseract/display_tesseract_trajectory - Value: "" - Trajectory Visualization: - Display Mode: Trail - Interrupt Display: false - Time Scale: 1 - Trail Step Size: 1 - Value: "" + Marker Topic: /tesseract/display_axes + Name: Target + Namespaces: + {} + Queue Size: 100 Value: true - - Class: tesseract_rviz/EnvironmentState + - Class: tesseract_rviz/TesseractWorkbench Enabled: true - Environment: - Alpha: 1 + Environment Properties: Display Mode: URDF - Show All Links: true - Show Collision: false - Show Highlights: true - Show Visual: true + Joint State Topic: /joint_states + Monitor Topic: /tesseract_environment + Snapshot Topic: /tesseract_environment_snapshot URDF Parameter: robot_description Value: "" - Joint State Monitor: - Topic: /joint_states - Value: "" - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Collision: - ACM: - floor: Never - gantry_axis_1: Never - gantry_axis_2: Adjacent - gantry_base: Never - human_estop_zone: Never - link_1: Adjacent - link_2: Never - link_3: Never - robot_estop_zone: Adjacent - Show Axes: false - Show Trail: false - Value: true - floor: - Alpha: 1 - Collision: - ACM: - base_link: Never - gantry_axis_1: Never - gantry_axis_2: Never - gantry_base: Adjacent - human_estop_zone: Adjacent - link_1: Never - link_2: Never - link_3: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - gantry_axis_1: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Never - gantry_axis_2: Adjacent - gantry_base: Adjacent - human_estop_zone: Never - link_1: Never - link_2: Never - link_3: Never - robot_estop_zone: Never - Show Axes: false - Show Trail: false - Value: true - gantry_axis_2: - Alpha: 1 - Collision: - ACM: - base_link: Adjacent - floor: Never - gantry_axis_1: Adjacent - gantry_base: Never - human_estop_zone: Never - link_1: Never - link_2: Never - link_3: Never - robot_estop_zone: Adjacent - Show Axes: false - Show Trail: false - Value: true - gantry_base: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Adjacent - gantry_axis_1: Adjacent - gantry_axis_2: Never - human_estop_zone: Default - link_1: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - gantry_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - human_estop_zone: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Adjacent - gantry_axis_1: Never - gantry_axis_2: Never - gantry_base: Default - link_1: Never - link_2: Default - link_3: Default - link_4: Default - link_5: Default - link_6: Default - Show Axes: false - Show Trail: false - Value: true - human_x: - Alpha: 1 - Show Axes: false - Show Trail: false - human_y: - Alpha: 1 - Show Axes: false - Show Trail: false - link_1: - Alpha: 1 - Collision: - ACM: - base_link: Adjacent - floor: Never - gantry_axis_1: Never - gantry_axis_2: Never - gantry_base: Never - human_estop_zone: Never - link_2: Adjacent - link_3: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_2: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Never - gantry_axis_1: Never - gantry_axis_2: Never - human_estop_zone: Default - link_1: Adjacent - link_3: Adjacent - link_4: Never - link_5: Never - link_6: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_3: - Alpha: 1 - Collision: - ACM: - base_link: Never - floor: Never - gantry_axis_1: Never - gantry_axis_2: Never - human_estop_zone: Default - link_1: Never - link_2: Adjacent - link_4: Adjacent - link_5: Never - link_6: Never - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_4: - Alpha: 1 - Collision: - ACM: - human_estop_zone: Default - link_2: Never - link_3: Adjacent - link_5: Adjacent - link_6: Default - robot_estop_zone: Default - Show Axes: false - Show Trail: false - Value: true - link_5: - Alpha: 1 - Collision: - ACM: - human_estop_zone: Default - link_2: Never - link_3: Never - link_4: Adjacent - link_6: Adjacent - robot_estop_zone: Default - Show Axes: false - Show Trail: false + Joint Trajectory Properties: + Legacy Joint Trajectory: Value: true - link_6: - Alpha: 1 - Collision: - ACM: - human_estop_zone: Default - link_2: Never - link_3: Never - link_4: Default - link_5: Adjacent - robot_estop_zone: Default - Show Axes: false - Show Trail: false + topic: /joint_trajectory + Tesseract Joint Trajectory: + Joint Trajectory Topic: /tesseract/display_tesseract_trajectory Value: true - robot_estop_zone: - Alpha: 1 - Collision: - ACM: - base_link: Adjacent - floor: Default - gantry_axis_1: Never - gantry_axis_2: Adjacent - gantry_base: Default - link_1: Default - link_2: Default - link_3: Default - link_4: Default - link_5: Default - link_6: Default - Show Axes: false - Show Trail: false - Value: true - target_x: - Alpha: 1 - Show Axes: false - Show Trail: false - target_y: - Alpha: 1 - Show Axes: false - Show Trail: false - target_z: - Alpha: 1 - Show Axes: false - Show Trail: false - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: EnvironmentState - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /tesseract/display_axes - Name: Target - Namespaces: - {} - Queue Size: 100 + Value: "" + Name: TesseractWorkbench Value: true + tesseract::EnvMonitorJointStateTopic: /joint_states + tesseract::EnvMonitorMode: URDF + tesseract::EnvMonitorSnapshotTopic: /tesseract_environment_snapshot + tesseract::EnvMonitorTopic: /tesseract_environment + tesseract::EnvMonitorURDFDescription: robot_description Enabled: true Global Options: Background Color: 48; 48; 48 @@ -625,7 +113,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 15.383088111877441 + Distance: 19.296546936035156 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -633,17 +121,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.23639477789402008 - Y: 4.130664348602295 - Z: -1.5134122371673584 + X: 0.5141215324401855 + Y: 3.9703733921051025 + Z: -1.8974144458770752 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4003981947898865 + Pitch: 0.6253979802131653 Target Frame: - Yaw: 6.198583602905273 + Yaw: 6.233575820922852 Saved: ~ Window Geometry: Displays: @@ -651,10 +139,12 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000024300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb00000038005400650073007300650072006100630074005400720061006a006500630074006f007200790020002d00200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010000000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004310000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001d50000037afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb00000038005400650073007300650072006100630074005400720061006a006500630074006f007200790020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb000000280054006500730073006500720061006300740045006e007600690072006f006e006d0065006e007400000002660000014c0000014c00ffffff00000001000002400000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d0000037a000003520100001cfa000000000100000002fb000000240054006500730073006500720061006300740057006f0072006b00620065006e006300680100000000ffffffff0000024000fffffffb0000000a005600690065007700730100000638000001000000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000039fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003170000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - TesseractTrajectory - Slider: + TesseractEnvironment: + collapsed: false + TesseractWorkbench: collapsed: false Time: collapsed: false @@ -662,6 +152,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 3840 + Width: 1848 + X: 3912 Y: 0 diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp index 6b242d3cd..53f8f50fa 100644 --- a/tesseract_rosutils/src/plotting.cpp +++ b/tesseract_rosutils/src/plotting.cpp @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -318,9 +320,11 @@ void ROSPlotting::plotTrajectory(const tesseract_msgs::Trajectory& traj, std::st void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj, const tesseract_scene_graph::StateSolver& /*state_solver*/, - std::string /*ns*/) + std::string ns) { tesseract_msgs::Trajectory msg; + msg.ns = ns; + msg.joint_trajectories_uuid = boost::uuids::to_string(traj.uuid); // Set the initial state for (std::size_t i = 0; i < traj[0].joint_names.size(); ++i) @@ -361,12 +365,7 @@ void ROSPlotting::plotTrajectory(const tesseract_environment::Environment& env, // Convert to joint trajectory assert(instruction.isCompositeInstruction()); const auto& ci = instruction.as(); - tesseract_common::JointTrajectory traj = tesseract_planning::toJointTrajectory(ci); - - // Set the joint trajectory message - tesseract_msgs::JointTrajectory traj_msg; - toMsg(traj_msg, traj); - msg.joint_trajectories.push_back(traj_msg); + msg.instructions = tesseract_common::Serialization::toArchiveStringXML(ci); plotTrajectory(msg); } diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp index 94e082bfd..f412d9994 100644 --- a/tesseract_rosutils/src/utils.cpp +++ b/tesseract_rosutils/src/utils.cpp @@ -1626,6 +1626,8 @@ void toMsg(const tesseract_msgs::EnvironmentStatePtr& state_msg, const tesseract void toMsg(tesseract_msgs::JointTrajectory& traj_msg, const tesseract_common::JointTrajectory& traj) { + traj_msg.uuid = boost::uuids::to_string(traj.uuid); + traj_msg.description = traj.description; for (const auto& js : traj) { assert(js.joint_names.size() == static_cast(js.position.size())); @@ -1653,6 +1655,8 @@ void toMsg(tesseract_msgs::JointTrajectory& traj_msg, const tesseract_common::Jo tesseract_common::JointTrajectory fromMsg(const tesseract_msgs::JointTrajectory& traj_msg) { tesseract_common::JointTrajectory trajectory; + trajectory.uuid = boost::lexical_cast(traj_msg.uuid); + trajectory.description = traj_msg.description; for (const auto& js_msg : traj_msg.states) { assert(js_msg.joint_names.size() == static_cast(js_msg.position.size())); diff --git a/tesseract_rviz/src/joint_trajectory_monitor_properties.cpp b/tesseract_rviz/src/joint_trajectory_monitor_properties.cpp index 28f0d9e53..d6296056b 100644 --- a/tesseract_rviz/src/joint_trajectory_monitor_properties.cpp +++ b/tesseract_rviz/src/joint_trajectory_monitor_properties.cpp @@ -12,8 +12,11 @@ #include #include +#include #include #include +#include +#include #include #include @@ -21,6 +24,8 @@ #include #include +#include +#include #include @@ -57,6 +62,8 @@ struct JointTrajectoryMonitorProperties::Implementation tesseract_common::JointTrajectorySet trajectory_set(initial_state); tesseract_common::JointTrajectory joint_trajectory = tesseract_rosutils::fromMsg(*msg); + trajectory_set.setUUID(joint_trajectory.uuid); + trajectory_set.setDescription(joint_trajectory.description); trajectory_set.appendJointTrajectory(joint_trajectory); tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set); QApplication::sendEvent(qApp, &event); @@ -93,12 +100,43 @@ struct JointTrajectoryMonitorProperties::Implementation if (!msg->ns.empty()) trajectory_set.setNamespace(msg->ns); - for (const auto& joint_trajectory_msg : msg->joint_trajectories) + if (!msg->instructions.empty()) { - tesseract_common::JointTrajectory joint_trajectory = tesseract_rosutils::fromMsg(joint_trajectory_msg); - trajectory_set.appendJointTrajectory(joint_trajectory); - tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set); - QApplication::sendEvent(qApp, &event); + auto ci = tesseract_common::Serialization::fromArchiveStringXML( + msg->instructions); + trajectory_set.setUUID(ci.getUUID()); + trajectory_set.setDescription(ci.getDescription()); + if (!ci.empty() && ci.front().isCompositeInstruction()) + { + for (const auto& entry : ci) + { + const auto& sub_ci = entry.as(); + tesseract_common::JointTrajectory joint_trajectory = tesseract_planning::toJointTrajectory(sub_ci); + trajectory_set.appendJointTrajectory(joint_trajectory); + } + + tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set); + QApplication::sendEvent(qApp, &event); + } + else + { + tesseract_common::JointTrajectory joint_trajectory = tesseract_planning::toJointTrajectory(ci); + trajectory_set.appendJointTrajectory(joint_trajectory); + tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set); + QApplication::sendEvent(qApp, &event); + } + } + else + { + trajectory_set.setUUID(boost::lexical_cast(msg->joint_trajectories_uuid)); + trajectory_set.setDescription(msg->joint_trajectories_description); + for (const auto& joint_trajectory_msg : msg->joint_trajectories) + { + tesseract_common::JointTrajectory joint_trajectory = tesseract_rosutils::fromMsg(joint_trajectory_msg); + trajectory_set.appendJointTrajectory(joint_trajectory); + tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set); + QApplication::sendEvent(qApp, &event); + } } } catch (...)