Skip to content

Commit

Permalink
Adding Trajopt_Ifopt option to all examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen committed Feb 23, 2024
1 parent c37a56f commit 86b5f25
Show file tree
Hide file tree
Showing 10 changed files with 61 additions and 6 deletions.
4 changes: 4 additions & 0 deletions tesseract_ros_examples/launch/car_seat_example.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
<arg name="robot_description" default="robot_description"/>
<arg name="plotting" default="true"/>
<arg name="rviz" default="true"/>
<arg name="ifopt" default="false"/>
<arg name="debug" default="false"/>
<arg name="testing" default="false"/>

<!-- Load universal robot description format (URDF) -->
Expand All @@ -19,6 +21,8 @@
<node pkg="tesseract_ros_examples" exec="tesseract_ros_examples_car_seat_example_node" name="tesseract_ros_examples_car_seat_example_node" output="screen">
<param name="plotting" type="bool" value="$(var plotting)"/>
<param name="rviz" type="bool" value="$(var rviz)"/>
<param name="ifopt" type="bool" value="$(var ifopt)"/>
<param name="debug" type="bool" value="$(var debug)"/>
<param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
<param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
</node>
Expand Down
4 changes: 4 additions & 0 deletions tesseract_ros_examples/launch/freespace_hybrid_example.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
<arg name="robot_description" default="robot_description"/>
<arg name="plotting" default="true"/>
<arg name="rviz" default="true"/>
<arg name="ifopt" default="false"/>
<arg name="debug" default="false"/>
<arg name="testing" default="false"/>
<arg name="range" default="0.01"/>
<arg name="planning_time" default="10.0"/>
Expand All @@ -22,6 +24,8 @@
<node pkg="tesseract_ros_examples" exec="tesseract_ros_examples_freespace_hybrid_example_node" output="screen">
<param name="plotting" type="bool" value="$(var plotting)"/>
<param name="rviz" type="bool" value="$(var rviz)"/>
<param name="ifopt" type="bool" value="$(var ifopt)"/>
<param name="debug" type="bool" value="$(var debug)"/>
<param name="range" type="float" value="$(var range)"/>
<param name="planning_time" type="float" value="$(var planning_time)"/>
<param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
Expand Down
6 changes: 5 additions & 1 deletion tesseract_ros_examples/launch/pick_and_place_example.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
<arg name="box_x" default="0.15"/>
<arg name="box_y" default="0.4"/>
<arg name="rviz" default="true"/>
<arg name="ifopt" default="false"/>
<arg name="debug" default="false"/>
<arg name="testing" default="false"/>

<!-- Load Robot Model -->
Expand All @@ -27,8 +29,10 @@
<param name="plotting" type="bool" value="$(var plotting)"/>
<param name="file_write_cb" value="$(var file_write_cb)" />
<param name="rviz" type="bool" value="$(var rviz)"/>
<param name="ifopt" type="bool" value="$(var ifopt)"/>
<param name="debug" type="bool" value="$(var debug)"/>
<param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
<param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
<param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
</node>

<!-- Launch visualization -->
Expand Down
4 changes: 4 additions & 0 deletions tesseract_ros_examples/launch/puzzle_piece_auxillary_axes_example.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
<arg name="robot_description" default="robot_description"/>
<arg name="plotting" default="true"/>
<arg name="rviz" default="true"/>
<arg name="ifopt" default="false"/>
<arg name="debug" default="false"/>
<arg name="testing" default="false"/>

<!-- Load universal robot description format (URDF) -->
Expand All @@ -20,6 +22,8 @@
<node pkg="tesseract_ros_examples" exec="tesseract_ros_examples_puzzle_piece_auxillary_axes_example_node" output="screen">
<param name="plotting" type="bool" value="$(var plotting)"/>
<param name="rviz" type="bool" value="$(var rviz)"/>
<param name="ifopt" type="bool" value="$(var ifopt)"/>
<param name="debug" type="bool" value="$(var debug)"/>
<param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
<param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
</node>
Expand Down
4 changes: 4 additions & 0 deletions tesseract_ros_examples/launch/puzzle_piece_example.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
<arg name="robot_description" default="robot_description"/>
<arg name="plotting" default="true"/>
<arg name="rviz" default="true"/>
<arg name="ifopt" default="false"/>
<arg name="debug" default="false"/>
<arg name="testing" default="false"/>

<!-- Load universal robot description format (URDF) -->
Expand All @@ -20,6 +22,8 @@
<node pkg="tesseract_ros_examples" exec="tesseract_ros_examples_puzzle_piece_example_node" output="screen">
<param name="plotting" type="bool" value="$(var plotting)"/>
<param name="rviz" type="bool" value="$(var rviz)"/>
<param name="ifopt" type="bool" value="$(var ifopt)"/>
<param name="debug" type="bool" value="$(var debug)"/>
<param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
<param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
</node>
Expand Down
9 changes: 8 additions & 1 deletion tesseract_ros_examples/src/car_seat_example_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,18 @@ int main(int argc, char** argv)
// Get ROS Parameters
bool plotting = node->declare_parameter("plotting", true);
bool rviz = node->declare_parameter("rviz", true);
bool ifopt = node->declare_parameter("ifopt", false);
bool debug = node->declare_parameter("debug", false);

// Initial setup
std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");

if (ifopt)
{
RCLCPP_INFO(node->get_logger(), "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))
Expand All @@ -72,7 +79,7 @@ int main(int argc, char** argv)
if (plotting)
plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());

CarSeatExample example(env, plotter);
CarSeatExample example(env, plotter, ifopt, debug);
rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));

example.run();
Expand Down
9 changes: 8 additions & 1 deletion tesseract_ros_examples/src/freespace_hybrid_example_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,13 +51,20 @@ int main(int argc, char** argv)
// Get ROS Parameters
bool plotting = node->declare_parameter("plotting", true);
bool rviz = node->declare_parameter("rviz", true);
bool ifopt = node->declare_parameter("ifopt", false);
bool debug = node->declare_parameter("debug", false);
double range = node->declare_parameter("range", 0.01);
double planning_time = node->declare_parameter("planning_time", 60.0);

// Initial setup
std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");

if (ifopt)
{
RCLCPP_INFO(node->get_logger(), "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))
Expand All @@ -74,7 +81,7 @@ int main(int argc, char** argv)
if (plotting)
plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());

FreespaceHybridExample example(env, plotter, range, planning_time);
FreespaceHybridExample example(env, plotter, ifopt, debug, range, planning_time);
rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));

example.run();
Expand Down
9 changes: 8 additions & 1 deletion tesseract_ros_examples/src/pick_and_place_example_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,17 @@ int main(int argc, char** argv)
// Get ROS Parameters
bool plotting = node->declare_parameter("plotting", true);
bool rviz = node->declare_parameter("rviz", true);
bool ifopt = node->declare_parameter("ifopt", false);
bool debug = node->declare_parameter("debug", false);

// Initial setup
std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");

if (ifopt)
{
RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");
}

auto env = std::make_shared<tesseract_environment::Environment>();
auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
Expand All @@ -72,7 +79,7 @@ int main(int argc, char** argv)
if (plotting)
plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());

PickAndPlaceExample example(env, plotter);
PickAndPlaceExample example(env, plotter, ifopt, debug);
rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));

example.run();
Expand Down
9 changes: 8 additions & 1 deletion tesseract_ros_examples/src/puzzle_piece_auxillary_axes_example_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,18 @@ int main(int argc, char** argv)
// Get ROS Parameters
bool plotting = node->declare_parameter("plotting", true);
bool rviz = node->declare_parameter("rviz", true);
bool ifopt = node->declare_parameter("ifopt", false);
bool debug = node->declare_parameter("debug", false);

// Initial setup
std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");

if (ifopt)
{
RCLCPP_INFO(node->get_logger(), "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))
Expand All @@ -72,7 +79,7 @@ int main(int argc, char** argv)
if (plotting)
plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());

PuzzlePieceAuxillaryAxesExample example(env, plotter);
PuzzlePieceAuxillaryAxesExample example(env, plotter, ifopt, debug);
rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));

example.run();
Expand Down
9 changes: 8 additions & 1 deletion tesseract_ros_examples/src/puzzle_piece_example_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,18 @@ int main(int argc, char** argv)
// Get ROS Parameters
bool plotting = node->declare_parameter("plotting", true);
bool rviz = node->declare_parameter("rviz", true);
bool ifopt = node->declare_parameter("ifopt", false);
bool debug = node->declare_parameter("debug", false);

// Initial setup
std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");

if (ifopt)
{
RCLCPP_INFO(node->get_logger(), "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))
Expand All @@ -72,7 +79,7 @@ int main(int argc, char** argv)
if (plotting)
plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());

PuzzlePieceExample example(env, plotter);
PuzzlePieceExample example(env, plotter, ifopt, debug);
rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));

example.run();
Expand Down

0 comments on commit 86b5f25

Please sign in to comment.