Skip to content

Commit

Permalink
Upgrade to tesseract version 0.20.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen committed Oct 6, 2023
1 parent 7397e48 commit 6307158
Show file tree
Hide file tree
Showing 5 changed files with 17 additions and 58 deletions.
6 changes: 3 additions & 3 deletions dependencies_with_ext.repos
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,19 +6,19 @@ repositories:
tesseract:
type: git
url: https://github.com/tesseract-robotics/tesseract.git
version: 0.19.2
version: 0.20.0
trajopt:
type: git
url: https://github.com/tesseract-robotics/trajopt.git
version: 0.6.1
tesseract_planning:
type: git
url: https://github.com/tesseract-robotics/tesseract_planning.git
version: 0.19.1
version: 0.20.0
tesseract_qt:
type: git
url: https://github.com/tesseract-robotics/tesseract_qt.git
version: 0.19.0
version: 0.20.0
descartes_light:
type: git
url: https://github.com/swri-robotics/descartes_light.git
Expand Down
20 changes: 2 additions & 18 deletions tesseract_planning_server/include/tesseract_planning_server/tesseract_planning_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <deque>
#include <shared_mutex>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <tf2_ros/buffer.h>
Expand All @@ -51,17 +49,9 @@ class TesseractPlanningServer

static const std::string DEFAULT_GET_MOTION_PLAN_ACTION; // "/tesseract_get_motion_plan"

TesseractPlanningServer(rclcpp::Node::SharedPtr node,
const std::string& robot_description,
std::string input_key,
std::string output_key,
std::string name);
TesseractPlanningServer(rclcpp::Node::SharedPtr node, const std::string& robot_description, std::string name);

TesseractPlanningServer(rclcpp::Node::SharedPtr node,
tesseract_environment::Environment::UPtr env,
std::string input_key,
std::string output_key,
std::string name);
TesseractPlanningServer(rclcpp::Node::SharedPtr node, tesseract_environment::Environment::UPtr env, std::string name);

~TesseractPlanningServer() = default;
TesseractPlanningServer(const TesseractPlanningServer&) = delete;
Expand Down Expand Up @@ -104,12 +94,6 @@ class TesseractPlanningServer
/** @brief The task planning server */
tesseract_planning::TaskComposerServer::UPtr planning_server_;

/** @brief The input key */
std::string input_key_;

/** @brief The output key */
std::string output_key_;

/** @brief The motion planning action server */
rclcpp_action::Server<tesseract_msgs::action::GetMotionPlan>::SharedPtr motion_plan_server_;

Expand Down
4 changes: 0 additions & 4 deletions tesseract_planning_server/launch/planning_server.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,6 @@
<arg name="cache_size" default="5"/>
<arg name="cache_refresh_rate" default="0.1"/>
<arg name="task_composer_config" default="$(find tesseract_task_composer)/config/task_composer_plugins.yaml"/>
<arg name="input_key" default="input_data"/>
<arg name="output_key" default="output_data"/>

<node pkg="tesseract_planning_server" type="tesseract_planning_server_node" name="tesseract_planning_server">
<param name="robot_description" type="string" value="$(arg robot_description)"/>
Expand All @@ -22,8 +20,6 @@
<param name="cache_size" type="int" value="$(arg cache_size)"/>
<param name="cache_refresh_rate" type="double" value="$(arg cache_refresh_rate)"/>
<param name="task_composer_config" type="string" value="$(arg task_composer_config)"/>
<param name="input_key" type="string" value="$(arg input_key)"/>
<param name="output_key" type="string" value="$(arg output_key)"/>
</node>

</launch>
29 changes: 10 additions & 19 deletions tesseract_planning_server/src/tesseract_planning_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_common/timer.h>

using tesseract_common::Serialization;
using tesseract_planning::InstructionPoly;
using tesseract_planning::PlanningTaskComposerProblem;
using tesseract_planning::TaskComposerDataStorage;
using tesseract_rosutils::processMsg;

static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask";
Expand All @@ -78,16 +81,12 @@ const std::string TesseractPlanningServer::DEFAULT_GET_MOTION_PLAN_ACTION = "tes

TesseractPlanningServer::TesseractPlanningServer(rclcpp::Node::SharedPtr node,
const std::string& robot_description,
std::string input_key,
std::string output_key,
std::string name)
: node_(node)
, monitor_(std::make_shared<tesseract_monitoring::ROSEnvironmentMonitor>(node_, robot_description, name))
, environment_cache_(std::make_shared<tesseract_environment::DefaultEnvironmentCache>(monitor_->getEnvironment()))
, profiles_(std::make_shared<tesseract_planning::ProfileDictionary>())
, planning_server_(std::make_unique<tesseract_planning::TaskComposerServer>())
, input_key_(std::move(input_key))
, output_key_(std::move(output_key))
, motion_plan_server_(rclcpp_action::create_server<tesseract_msgs::action::GetMotionPlan>(
node_,
DEFAULT_GET_MOTION_PLAN_ACTION,
Expand All @@ -102,16 +101,12 @@ TesseractPlanningServer::TesseractPlanningServer(rclcpp::Node::SharedPtr node,

TesseractPlanningServer::TesseractPlanningServer(rclcpp::Node::SharedPtr node,
tesseract_environment::Environment::UPtr env,
std::string input_key,
std::string output_key,
std::string name)
: node_(node)
, monitor_(std::make_shared<tesseract_monitoring::ROSEnvironmentMonitor>(node_, std::move(env), name))
, environment_cache_(std::make_shared<tesseract_environment::DefaultEnvironmentCache>(monitor_->getEnvironment()))
, profiles_(std::make_shared<tesseract_planning::ProfileDictionary>())
, planning_server_(std::make_unique<tesseract_planning::TaskComposerServer>())
, input_key_(std::move(input_key))
, output_key_(std::move(output_key))
, motion_plan_server_(rclcpp_action::create_server<tesseract_msgs::action::GetMotionPlan>(
node_,
DEFAULT_GET_MOTION_PLAN_ACTION,
Expand Down Expand Up @@ -209,20 +204,17 @@ void TesseractPlanningServer::onMotionPlanningCallback(
}

auto problem = std::make_unique<tesseract_planning::PlanningTaskComposerProblem>(goal->request.name);
auto data_storage = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
try
{
auto ci = Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(goal->request.instructions)
.as<tesseract_planning::CompositeInstruction>();
data_storage->setData(input_key_, ci);
problem->input_instruction =
Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(goal->request.instructions);
}
catch (const std::exception& e)
{
result->response.successful = false;
std::ostringstream oss;
oss << "Failed to deserialize program instruction with error: '" << e.what() << "'!" << std::endl;
oss << " Make sure the program was serialized from an Instruction type and not a CompositeInstruction type."
<< std::endl;
oss << " Make sure the program was serialized from an InstructionPoly type." << std::endl;
RCLCPP_ERROR_STREAM(node_->get_logger(), oss.str());
goal_handle->succeed(result);
return;
Expand Down Expand Up @@ -251,7 +243,7 @@ void TesseractPlanningServer::onMotionPlanningCallback(
tesseract_common::Timer timer;
timer.start();
tesseract_planning::TaskComposerFuture::UPtr plan_future =
planning_server_->run(std::move(problem), std::move(data_storage), executor_name);
planning_server_->run(std::move(problem), std::make_unique<TaskComposerDataStorage>(), executor_name);
plan_future->wait(); // Wait for results
timer.stop();

Expand All @@ -260,10 +252,8 @@ void TesseractPlanningServer::onMotionPlanningCallback(
{
try
{
// Get Task
const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);

// Save dot graph
const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
std::stringstream dotgraph;
task.dump(dotgraph, nullptr, plan_future->context->task_infos.getInfoMap());
result->response.dotgraph = dotgraph.str();
Expand All @@ -278,7 +268,8 @@ void TesseractPlanningServer::onMotionPlanningCallback(

try
{
tesseract_common::AnyPoly results = plan_future->context->data_storage->getData(output_key_);
const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
tesseract_common::AnyPoly results = plan_future->context->data_storage->getData(task.getOutputKeys().front());
result->response.results = Serialization::toArchiveStringXML<tesseract_planning::InstructionPoly>(
results.as<tesseract_planning::CompositeInstruction>());
}
Expand Down
16 changes: 2 additions & 14 deletions tesseract_planning_server/src/tesseract_planning_server_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_planning_server/tesseract_planning_server.h>

using namespace tesseract_environment;
using tesseract_planning_server::TesseractPlanningServer;

const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */
const std::string ROBOT_DESCRIPTION_SEMANTIC_PARAM = "robot_description_semantic";
Expand All @@ -57,18 +58,6 @@ int main(int argc, char** argv)
RCLCPP_ERROR(node->get_logger(), "Missing required parameter monitor_namespace!");
return 1;
}
std::string input_key = node->declare_parameter("input_key", "");
if (input_key.empty())
{
RCLCPP_ERROR(node->get_logger(), "Missing required parameter input_key!");
return 1;
}
std::string output_key = node->declare_parameter("output_key", "");
if (output_key.empty())
{
RCLCPP_ERROR(node->get_logger(), "Missing required parameter output_key!");
return 1;
}
std::string monitored_namespace = node->declare_parameter("monitored_namespace", "");
std::string robot_description = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
if (robot_description.empty())
Expand All @@ -87,8 +76,7 @@ int main(int argc, char** argv)
cache_refresh_rate = node->declare_parameter("cache_refresh_rate", cache_refresh_rate);
std::string task_composer_config = node->declare_parameter("task_composer_config", "");

planning_server = std::make_shared<tesseract_planning_server::TesseractPlanningServer>(
node, ROBOT_DESCRIPTION_PARAM, input_key, output_key, monitor_namespace);
planning_server = std::make_shared<TesseractPlanningServer>(node, ROBOT_DESCRIPTION_PARAM, monitor_namespace);

planning_server->getEnvironmentCache().setCacheSize(cache_size);

Expand Down

0 comments on commit 6307158

Please sign in to comment.