From d39a597245c632fb3487514605f3c33bf3ec823b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 3 Apr 2023 15:54:24 -0500 Subject: [PATCH] Add support for AddTrajectoryLinkCommand --- .../src/basic_cartesian_example_node.cpp | 5 +++++ .../src/glass_upright_example_node.cpp | 5 +++++ tesseract_rosutils/src/utils.cpp | 13 +++++++++++-- 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/tesseract_ros_examples/src/basic_cartesian_example_node.cpp b/tesseract_ros_examples/src/basic_cartesian_example_node.cpp index a7227cb40..c5b2be98d 100644 --- a/tesseract_ros_examples/src/basic_cartesian_example_node.cpp +++ b/tesseract_ros_examples/src/basic_cartesian_example_node.cpp @@ -67,6 +67,11 @@ int main(int argc, char** argv) nh.getParam(ROBOT_DESCRIPTION_PARAM, urdf_xml_string); nh.getParam(ROBOT_SEMANTIC_PARAM, srdf_xml_string); + if (ifopt == true) + { + ROS_INFO("Using TrajOpt Ifopt!"); + } + auto env = std::make_shared(); auto locator = std::make_shared(); if (!env->init(urdf_xml_string, srdf_xml_string, locator)) diff --git a/tesseract_ros_examples/src/glass_upright_example_node.cpp b/tesseract_ros_examples/src/glass_upright_example_node.cpp index dbf5f113c..62ffb3278 100644 --- a/tesseract_ros_examples/src/glass_upright_example_node.cpp +++ b/tesseract_ros_examples/src/glass_upright_example_node.cpp @@ -57,6 +57,11 @@ int main(int argc, char** argv) pnh.param("ifopt", ifopt, ifopt); pnh.param("debug", debug, debug); + if (ifopt == true) + { + ROS_INFO("Using TrajOpt Ifopt!"); + } + // Initial setup std::string urdf_xml_string, srdf_xml_string; nh.getParam(ROBOT_DESCRIPTION_PARAM, urdf_xml_string); diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp index 8d4b401e3..aa4bc9d48 100644 --- a/tesseract_rosutils/src/utils.cpp +++ b/tesseract_rosutils/src/utils.cpp @@ -696,8 +696,13 @@ bool fromMsg(tesseract_scene_graph::Visual::Ptr& visual, const tesseract_msgs::V visual = std::make_shared(); visual->name = visual_msg.name; tf::poseMsgToEigen(visual_msg.origin, visual->origin); - fromMsg(visual->geometry, visual_msg.geometry); + + tesseract_geometry::Geometry::Ptr geom; + fromMsg(geom, visual_msg.geometry); + visual->geometry = geom; + fromMsg(visual->material, visual_msg.material); + return true; } @@ -714,7 +719,11 @@ bool fromMsg(tesseract_scene_graph::Collision::Ptr& collision, const tesseract_m collision = std::make_shared(); collision->name = collision_msg.name; tf::poseMsgToEigen(collision_msg.origin, collision->origin); - fromMsg(collision->geometry, collision_msg.geometry); + + tesseract_geometry::Geometry::Ptr geom; + fromMsg(geom, collision_msg.geometry); + collision->geometry = geom; + return true; }