Add support for AddTrajectoryLinkCommand

tesseract-robotics · Apr 4, 2023 · d39a597 · d39a597
1 parent 16c16d4
commit d39a597
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 2 deletions.
diff --git 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<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   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
@@ -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
@@ -696,8 +696,13 @@ bool fromMsg(tesseract_scene_graph::Visual::Ptr& visual, const tesseract_msgs::V
   visual = std::make_shared<tesseract_scene_graph::Visual>();
   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<tesseract_scene_graph::Collision>();
   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;
 }