mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
split repo into different ROS packages: msgs, core, visualization
This commit is contained in:
parent
3f93d6c9c3
commit
9c5ddd3f6d
@ -1,49 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.6.12)
|
||||
project(moveit_task_constructor)
|
||||
|
||||
set(MSG_DEPS moveit_msgs visualization_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
genmsg ${MSG_DEPS}
|
||||
moveit_core
|
||||
moveit_ros_planning
|
||||
moveit_ros_planning_interface
|
||||
moveit_msgs
|
||||
moveit_ros_visualization
|
||||
eigen_conversions
|
||||
rviz
|
||||
)
|
||||
|
||||
# ROS messages, services and actions
|
||||
add_message_files(DIRECTORY msg
|
||||
FILES
|
||||
StageDescription.msg
|
||||
StageStatistics.msg
|
||||
TaskDescription.msg
|
||||
TaskStatistics.msg
|
||||
Solution.msg
|
||||
SubSolution.msg
|
||||
SubTrajectory.msg
|
||||
)
|
||||
add_service_files(DIRECTORY srv
|
||||
FILES
|
||||
GetSolution.srv
|
||||
)
|
||||
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME})
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
add_subdirectory(src)
|
||||
add_subdirectory(visualization)
|
||||
|
||||
install(DIRECTORY include/ DESTINATION include)
|
||||
install(FILES
|
||||
motion_planning_tasks_rviz_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
33
core/CMakeLists.txt
Normal file
33
core/CMakeLists.txt
Normal file
@ -0,0 +1,33 @@
|
||||
cmake_minimum_required(VERSION 2.6.12)
|
||||
project(moveit_task_constructor_core)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
moveit_core
|
||||
moveit_ros_planning
|
||||
moveit_ros_planning_interface
|
||||
moveit_task_constructor_msgs
|
||||
eigen_conversions
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES
|
||||
moveit_task_constructor_core
|
||||
moveit_task_constructor_core_stages
|
||||
INCLUDE_DIRS
|
||||
include
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
add_subdirectory(src)
|
||||
add_subdirectory(demo)
|
||||
add_subdirectory(test)
|
||||
|
||||
install(DIRECTORY include/ DESTINATION include)
|
||||
install(FILES
|
||||
motion_planning_stages_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
@ -1,10 +1,10 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit_task_constructor/stages/move.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/gripper.h>
|
||||
#include <moveit/task_constructor/stages/move.h>
|
||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/CollisionObject.h>
|
@ -1,10 +1,10 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit_task_constructor/stages/move.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/gripper.h>
|
||||
#include <moveit/task_constructor/stages/move.h>
|
||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include "stage_p.h"
|
||||
|
||||
#include <map>
|
||||
@ -71,7 +71,7 @@ public:
|
||||
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
||||
{}
|
||||
/// append all subsolutions to solution
|
||||
void fillMessage(::moveit_task_constructor::Solution &msg, Introspection *introspection) const override;
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override;
|
||||
|
||||
private:
|
||||
/// series of sub solutions
|
||||
@ -123,7 +123,7 @@ public:
|
||||
explicit WrappedSolution(StagePrivate* creator, SolutionBase* wrapped)
|
||||
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
||||
{}
|
||||
void fillMessage(::moveit_task_constructor::Solution &solution,
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||
Introspection* introspection = nullptr) const override {
|
||||
wrapped_->fillMessage(solution, introspection);
|
||||
}
|
@ -1,10 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/TaskDescription.h>
|
||||
#include <moveit_task_constructor/TaskStatistics.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit_task_constructor/GetSolution.h>
|
||||
#include <moveit_task_constructor_msgs/TaskDescription.h>
|
||||
#include <moveit_task_constructor_msgs/TaskStatistics.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/GetSolution.h>
|
||||
|
||||
#define DESCRIPTION_TOPIC "description"
|
||||
#define STATISTICS_TOPIC "statistics"
|
||||
@ -31,12 +31,12 @@ public:
|
||||
~Introspection();
|
||||
|
||||
/// fill task description message for publishing the task configuration
|
||||
moveit_task_constructor::TaskDescription& fillTaskDescription(moveit_task_constructor::TaskDescription& msg);
|
||||
moveit_task_constructor_msgs::TaskDescription& fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg);
|
||||
/// publish detailed task description
|
||||
void publishTaskDescription();
|
||||
|
||||
/// fill task state message for publishing the current task state
|
||||
moveit_task_constructor::TaskStatistics& fillTaskStatistics(moveit_task_constructor::TaskStatistics& msg);
|
||||
moveit_task_constructor_msgs::TaskStatistics& fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg);
|
||||
/// publish the current state of task
|
||||
void publishTaskState();
|
||||
|
||||
@ -52,8 +52,8 @@ public:
|
||||
void publishAllSolutions(bool wait = true);
|
||||
|
||||
/// get solution
|
||||
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||
moveit_task_constructor::GetSolution::Response &res);
|
||||
bool getSolution(moveit_task_constructor_msgs::GetSolution::Request &req,
|
||||
moveit_task_constructor_msgs::GetSolution::Response &res);
|
||||
|
||||
/// retrieve id of given stage
|
||||
uint32_t stageId(const moveit::task_constructor::Stage * const s) const;
|
||||
@ -62,7 +62,7 @@ public:
|
||||
uint32_t solutionId(const moveit::task_constructor::SolutionBase &s);
|
||||
|
||||
private:
|
||||
void fillStageStatistics(const Stage &stage, moveit_task_constructor::StageStatistics &s);
|
||||
void fillStageStatistics(const Stage &stage, moveit_task_constructor_msgs::StageStatistics &s);
|
||||
/// retrieve or set id of given stage
|
||||
uint32_t stageId(const moveit::task_constructor::Stage * const s);
|
||||
};
|
@ -5,7 +5,7 @@
|
||||
|
||||
#include "utils.h"
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <vector>
|
||||
#include <list>
|
||||
|
@ -2,8 +2,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
// define pimpl() functions accessing correctly casted pimpl_ pointer
|
||||
#define PIMPL_FUNCTIONS(Class) \
|
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
namespace moveit { namespace planning_interface {
|
||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
|
@ -3,7 +3,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
|
||||
#include <list>
|
||||
@ -140,7 +140,7 @@ public:
|
||||
void setCost(double cost);
|
||||
|
||||
/// append this solution to Solution msg
|
||||
virtual void fillMessage(::moveit_task_constructor::Solution &solution,
|
||||
virtual void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||
Introspection* introspection = nullptr) const = 0;
|
||||
|
||||
protected:
|
||||
@ -172,7 +172,7 @@ public:
|
||||
const std::string& name() const { return name_; }
|
||||
void setName(const std::string& name) { name_ = name; }
|
||||
|
||||
void fillMessage(::moveit_task_constructor::Solution &msg,
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
Introspection* introspection = nullptr) const override;
|
||||
|
||||
private:
|
@ -5,8 +5,8 @@
|
||||
|
||||
#include "container.h"
|
||||
|
||||
#include <moveit_task_constructor/introspection.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
@ -1,4 +1,4 @@
|
||||
<library path="libmoveit_task_constructor_stages">
|
||||
<library path="libmoveit_task_constructor_core_stages">
|
||||
<class name="moveit_task_constructor/Serial Container"
|
||||
type="moveit::task_constructor::SerialContainer"
|
||||
base_class_type="moveit::task_constructor::Stage">
|
@ -1,5 +1,5 @@
|
||||
<package>
|
||||
<name>moveit_task_constructor</name>
|
||||
<name>moveit_task_constructor_core</name>
|
||||
<version>0.0.0</version>
|
||||
<description>MoveIt Task Pipeline</description>
|
||||
|
||||
@ -12,17 +12,16 @@
|
||||
<build_depend>moveit_core</build_depend>
|
||||
<build_depend>moveit_ros_planning</build_depend>
|
||||
<build_depend>moveit_ros_planning_interface</build_depend>
|
||||
<build_depend>moveit_msgs</build_depend>
|
||||
<build_depend>rviz</build_depend>
|
||||
<build_depend>moveit_task_constructor_msgs</build_depend>
|
||||
|
||||
<run_depend>moveit_core</run_depend>
|
||||
<run_depend>moveit_msgs</run_depend>
|
||||
<run_depend>rviz</run_depend>
|
||||
<run_depend>moveit_ros_planning</run_depend>
|
||||
<run_depend>moveit_ros_planning_interface</run_depend>
|
||||
<run_depend>moveit_task_constructor_msgs</run_depend>
|
||||
|
||||
<test_depend>rostest</test_depend>
|
||||
|
||||
<export>
|
||||
<rviz plugin="${prefix}/motion_planning_tasks_rviz_plugin_description.xml"/>
|
||||
<moveit_task_constructor plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>
|
||||
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>
|
||||
</export>
|
||||
</package>
|
@ -5,25 +5,20 @@ add_library(${PROJECT_NAME}
|
||||
task.cpp
|
||||
introspection.cpp
|
||||
|
||||
stage_p.h
|
||||
container_p.h
|
||||
|
||||
${PROJECT_INCLUDE}/utils.h
|
||||
${PROJECT_INCLUDE}/storage.h
|
||||
${PROJECT_INCLUDE}/stage.h
|
||||
${PROJECT_INCLUDE}/stage_p.h
|
||||
${PROJECT_INCLUDE}/container.h
|
||||
${PROJECT_INCLUDE}/container_p.h
|
||||
${PROJECT_INCLUDE}/task.h
|
||||
${PROJECT_INCLUDE}/introspection.h
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
)
|
||||
|
||||
add_subdirectory(stages)
|
||||
add_subdirectory(demo)
|
||||
add_subdirectory(test)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
@ -1,6 +1,6 @@
|
||||
#include "container_p.h"
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
|
||||
#include <moveit_task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <memory>
|
||||
@ -422,10 +422,10 @@ bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
|
||||
return result;
|
||||
}
|
||||
|
||||
void SerialSolution::fillMessage(::moveit_task_constructor::Solution &msg,
|
||||
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
Introspection* introspection = nullptr) const
|
||||
{
|
||||
::moveit_task_constructor::SubSolution sub_msg;
|
||||
moveit_task_constructor_msgs::SubSolution sub_msg;
|
||||
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
|
||||
sub_msg.cost = this->cost();
|
||||
|
@ -1,7 +1,7 @@
|
||||
#include "stage_p.h"
|
||||
#include <moveit_task_constructor/introspection.h>
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
#include <ros/node_handle.h>
|
||||
#include <ros/publisher.h>
|
||||
@ -19,9 +19,9 @@ public:
|
||||
, task_(task)
|
||||
{
|
||||
resetMaps();
|
||||
task_description_publisher_ = nh_.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
|
||||
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
||||
solution_publisher_ = nh_.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
||||
task_description_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
|
||||
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1);
|
||||
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true);
|
||||
}
|
||||
void resetMaps () {
|
||||
// reset maps
|
||||
@ -44,7 +44,7 @@ public:
|
||||
ros::ServiceServer get_solution_service_;
|
||||
|
||||
/// mapping from stages to their id
|
||||
std::map<const void*, moveit_task_constructor::StageStatistics::_id_type> stage_to_id_map_;
|
||||
std::map<const void*, moveit_task_constructor_msgs::StageStatistics::_id_type> stage_to_id_map_;
|
||||
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
|
||||
};
|
||||
|
||||
@ -61,20 +61,20 @@ Introspection::~Introspection()
|
||||
|
||||
void Introspection::publishTaskDescription()
|
||||
{
|
||||
::moveit_task_constructor::TaskDescription msg;
|
||||
::moveit_task_constructor_msgs::TaskDescription msg;
|
||||
impl->task_description_publisher_.publish(fillTaskDescription(msg));
|
||||
}
|
||||
|
||||
void Introspection::publishTaskState()
|
||||
{
|
||||
::moveit_task_constructor::TaskStatistics msg;
|
||||
::moveit_task_constructor_msgs::TaskStatistics msg;
|
||||
impl->task_statistics_publisher_.publish(fillTaskStatistics(msg));
|
||||
}
|
||||
|
||||
void Introspection::reset()
|
||||
{
|
||||
// send empty task description message to indicate reset
|
||||
::moveit_task_constructor::TaskDescription msg;
|
||||
::moveit_task_constructor_msgs::TaskDescription msg;
|
||||
msg.id = impl->task_.id();
|
||||
impl->task_description_publisher_.publish(msg);
|
||||
|
||||
@ -83,7 +83,7 @@ void Introspection::reset()
|
||||
|
||||
void Introspection::publishSolution(const SolutionBase &s)
|
||||
{
|
||||
::moveit_task_constructor::Solution msg;
|
||||
moveit_task_constructor_msgs::Solution msg;
|
||||
s.fillMessage(msg, this);
|
||||
msg.task_id = impl->task_.id();
|
||||
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
|
||||
@ -92,7 +92,7 @@ void Introspection::publishSolution(const SolutionBase &s)
|
||||
|
||||
void Introspection::publishAllSolutions(bool wait)
|
||||
{
|
||||
::moveit_task_constructor::Solution msg;
|
||||
moveit_task_constructor_msgs::Solution msg;
|
||||
|
||||
Stage::SolutionProcessor processor = [this, &msg, wait](const SolutionBase& s) {
|
||||
std::cout << "publishing solution with cost: " << s.cost() << std::endl;
|
||||
@ -115,8 +115,8 @@ void Introspection::publishAllSolutions(bool wait)
|
||||
impl->task_.processSolutions(processor);
|
||||
}
|
||||
|
||||
bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||
moveit_task_constructor::GetSolution::Response &res)
|
||||
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request &req,
|
||||
moveit_task_constructor_msgs::GetSolution::Response &res)
|
||||
{
|
||||
auto it = impl->id_solution_bimap_.left.find(req.solution_id);
|
||||
if (it == impl->id_solution_bimap_.left.end())
|
||||
@ -145,7 +145,7 @@ uint32_t Introspection::solutionId(const SolutionBase& s)
|
||||
return result.first->first;
|
||||
}
|
||||
|
||||
void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor::StageStatistics& s)
|
||||
void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s)
|
||||
{
|
||||
// successfull solutions
|
||||
Stage::SolutionProcessor solutionProcessor = [this, &s](const SolutionBase& solution) {
|
||||
@ -162,13 +162,13 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc
|
||||
stage.processFailures(solutionProcessor);
|
||||
}
|
||||
|
||||
moveit_task_constructor::TaskDescription& Introspection::fillTaskDescription(moveit_task_constructor::TaskDescription &msg)
|
||||
moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription &msg)
|
||||
{
|
||||
ContainerBase::StageCallback stageProcessor =
|
||||
[this, &msg](const Stage& stage, int) -> bool {
|
||||
// this method is called for each child stage of a given parent
|
||||
moveit_task_constructor::StageDescription desc;
|
||||
moveit_task_constructor::StageStatistics stat;
|
||||
moveit_task_constructor_msgs::StageDescription desc;
|
||||
moveit_task_constructor_msgs::StageStatistics stat;
|
||||
desc.id = stat.id = stageId(&stage);
|
||||
|
||||
desc.name = stage.name();
|
||||
@ -195,12 +195,12 @@ moveit_task_constructor::TaskDescription& Introspection::fillTaskDescription(mov
|
||||
return msg;
|
||||
}
|
||||
|
||||
moveit_task_constructor::TaskStatistics& Introspection::fillTaskStatistics(moveit_task_constructor::TaskStatistics &msg)
|
||||
moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics &msg)
|
||||
{
|
||||
ContainerBase::StageCallback stageProcessor =
|
||||
[this, &msg](const Stage& stage, int) -> bool {
|
||||
// this method is called for each child stage of a given parent
|
||||
moveit_task_constructor::StageStatistics stat; // create new Stage msg
|
||||
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg
|
||||
stat.id = stageId(&stage);
|
||||
|
||||
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent());
|
@ -1,5 +1,5 @@
|
||||
#include "stage_p.h"
|
||||
#include "container_p.h"
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <ros/console.h>
|
@ -1,5 +1,5 @@
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
@ -1,5 +1,5 @@
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
@ -1,5 +1,5 @@
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/DisplayRobotState.h>
|
@ -1,6 +1,6 @@
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/stages/gripper.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
@ -1,6 +1,6 @@
|
||||
#include <moveit_task_constructor/stages/move.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/stages/move.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
@ -1,6 +1,6 @@
|
||||
#include "stage_p.h"
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit_task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
@ -52,10 +52,10 @@ void SolutionBase::setCost(double cost) {
|
||||
}
|
||||
|
||||
|
||||
void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg,
|
||||
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
Introspection *introspection) const {
|
||||
msg.sub_trajectory.emplace_back();
|
||||
moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back();
|
||||
moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back();
|
||||
t.id = introspection ? introspection->solutionId(*this) : 0;
|
||||
t.cost = this->cost();
|
||||
t.name = this->name();
|
@ -1,8 +1,8 @@
|
||||
#include "container_p.h"
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include <moveit_task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
@ -4,18 +4,11 @@
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-container
|
||||
${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
gtest_main)
|
||||
|
||||
add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-task_model
|
||||
motion_planning_tasks_rviz_plugin
|
||||
${PROJECT_NAME}_stages
|
||||
gtest_main)
|
||||
endif()
|
||||
|
||||
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
@ -1,5 +1,5 @@
|
||||
#include <container_p.h>
|
||||
#include <stage_p.h>
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <initializer_list>
|
@ -1,9 +1,9 @@
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
@ -1,8 +1,8 @@
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
@ -1,5 +1,5 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/CollisionObject.h>
|
@ -1,9 +1,9 @@
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/gripper.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
26
msgs/CMakeLists.txt
Normal file
26
msgs/CMakeLists.txt
Normal file
@ -0,0 +1,26 @@
|
||||
cmake_minimum_required(VERSION 2.6.12)
|
||||
project(moveit_task_constructor_msgs)
|
||||
|
||||
set(MSG_DEPS moveit_msgs visualization_msgs)
|
||||
|
||||
find_package(catkin REQUIRED genmsg ${MSG_DEPS})
|
||||
|
||||
# ROS messages, services and actions
|
||||
add_message_files(DIRECTORY msg
|
||||
FILES
|
||||
StageDescription.msg
|
||||
StageStatistics.msg
|
||||
TaskDescription.msg
|
||||
TaskStatistics.msg
|
||||
Solution.msg
|
||||
SubSolution.msg
|
||||
SubTrajectory.msg
|
||||
)
|
||||
add_service_files(DIRECTORY srv
|
||||
FILES
|
||||
GetSolution.srv
|
||||
)
|
||||
|
||||
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
||||
|
||||
catkin_package(DEPENDS ${MSG_DEPS})
|
16
msgs/package.xml
Normal file
16
msgs/package.xml
Normal file
@ -0,0 +1,16 @@
|
||||
<package>
|
||||
<name>moveit_task_constructor_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Messages for MoveIt Task Pipeline</description>
|
||||
|
||||
<license>BSD</license>
|
||||
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>moveit_msgs</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
|
||||
<run_depend>moveit_msgs</run_depend>
|
||||
<run_depend>visualization_msgs</run_depend>
|
||||
</package>
|
@ -1,3 +1,13 @@
|
||||
cmake_minimum_required(VERSION 2.6.12)
|
||||
project(moveit_task_constructor_visualization)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
moveit_task_constructor_msgs
|
||||
moveit_task_constructor_core
|
||||
moveit_ros_visualization
|
||||
rviz
|
||||
)
|
||||
|
||||
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile
|
||||
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
|
||||
|
||||
@ -20,5 +30,17 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||
set(CMAKE_AUTOMOC ON)
|
||||
add_definitions(-DQT_NO_KEYWORDS)
|
||||
|
||||
catkin_package(
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
add_subdirectory(visualization_tools)
|
||||
add_subdirectory(motion_planning_tasks)
|
||||
|
||||
install(DIRECTORY include/ DESTINATION include)
|
||||
install(FILES
|
||||
motion_planning_tasks_rviz_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
@ -1,31 +1,2 @@
|
||||
set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin)
|
||||
|
||||
qt_wrap_ui(UIC_FILES task_panel.ui)
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME}
|
||||
job_queue.cpp
|
||||
local_task_model.cpp
|
||||
remote_task_model.cpp
|
||||
task_list_model.cpp
|
||||
task_list_model_cache.cpp
|
||||
task_panel.cpp
|
||||
task_panel_p.h
|
||||
task_display.cpp
|
||||
pluginlib_factory.h
|
||||
factory_model.cpp
|
||||
plugin_init.cpp
|
||||
${UIC_FILES}
|
||||
)
|
||||
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
|
||||
target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
moveit_task_visualization_tools ${PROJECT_NAME}
|
||||
${catkin_LIBRARIES} ${QT_LIBRARIES}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
|
||||
)
|
||||
|
||||
install(TARGETS ${MOVEIT_LIB_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
add_subdirectory(src)
|
||||
add_subdirectory(test)
|
||||
|
31
visualization/motion_planning_tasks/src/CMakeLists.txt
Normal file
31
visualization/motion_planning_tasks/src/CMakeLists.txt
Normal file
@ -0,0 +1,31 @@
|
||||
set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin)
|
||||
|
||||
qt_wrap_ui(UIC_FILES task_panel.ui)
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME}
|
||||
job_queue.cpp
|
||||
local_task_model.cpp
|
||||
remote_task_model.cpp
|
||||
task_list_model.cpp
|
||||
task_list_model_cache.cpp
|
||||
task_panel.cpp
|
||||
task_panel_p.h
|
||||
task_display.cpp
|
||||
pluginlib_factory.h
|
||||
factory_model.cpp
|
||||
plugin_init.cpp
|
||||
${UIC_FILES}
|
||||
)
|
||||
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
|
||||
target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
moveit_task_visualization_tools
|
||||
${catkin_LIBRARIES} ${QT_LIBRARIES}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
|
||||
)
|
||||
|
||||
install(TARGETS ${MOVEIT_LIB_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
@ -36,7 +36,7 @@
|
||||
|
||||
#include "local_task_model.h"
|
||||
#include "factory_model.h"
|
||||
#include <container_p.h>
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
|
@ -37,7 +37,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "task_list_model.h"
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
@ -37,7 +37,7 @@
|
||||
#include <stdio.h>
|
||||
|
||||
#include "remote_task_model.h"
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <QApplication>
|
||||
@ -193,7 +193,7 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
|
||||
return true;
|
||||
}
|
||||
|
||||
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg)
|
||||
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_description_type &msg)
|
||||
{
|
||||
// iterate over descriptions and create new / update existing nodes where needed
|
||||
for (const auto &s : msg) {
|
||||
@ -246,7 +246,7 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::Ta
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg)
|
||||
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg)
|
||||
{
|
||||
}
|
||||
|
@ -62,8 +62,8 @@ public:
|
||||
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
|
||||
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;
|
||||
|
||||
void processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg);
|
||||
void processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg);
|
||||
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_description_type &msg);
|
||||
void processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg);
|
||||
};
|
||||
|
||||
}
|
@ -38,8 +38,8 @@
|
||||
|
||||
#include "task_display.h"
|
||||
#include "task_list_model_cache.h"
|
||||
#include <moveit_task_constructor/introspection.h>
|
||||
#include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||
|
||||
#include <moveit/rdf_loader/rdf_loader.h>
|
||||
#include <moveit/robot_model/robot_model.h>
|
||||
@ -59,7 +59,7 @@ TaskDisplay::TaskDisplay() : Display()
|
||||
|
||||
task_solution_topic_property_ =
|
||||
new rviz::RosTopicProperty("Task Solution Topic", "",
|
||||
ros::message_traits::datatype<moveit_task_constructor::Solution>(),
|
||||
ros::message_traits::datatype<moveit_task_constructor_msgs::Solution>(),
|
||||
"The topic on which task solutions (moveit_msgs::Solution messages) are received",
|
||||
this, SLOT(changedTaskSolutionTopic()), this);
|
||||
|
||||
@ -143,27 +143,27 @@ void TaskDisplay::changedRobotDescription()
|
||||
loadRobotModel();
|
||||
}
|
||||
|
||||
void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_constructor::TaskDescription> &event)
|
||||
void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_constructor_msgs::TaskDescription> &event)
|
||||
{
|
||||
const moveit_task_constructor::TaskDescriptionConstPtr& msg = event.getMessage();
|
||||
const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
task_list_model_->processTaskDescriptionMessage(id, *msg);
|
||||
});
|
||||
}
|
||||
|
||||
void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor::TaskStatistics> &event)
|
||||
void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor_msgs::TaskStatistics> &event)
|
||||
{
|
||||
const moveit_task_constructor::TaskStatisticsConstPtr& msg = event.getMessage();
|
||||
const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
task_list_model_->processTaskStatisticsMessage(id, *msg);
|
||||
});
|
||||
}
|
||||
|
||||
void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_constructor::Solution> &event)
|
||||
void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_constructor_msgs::Solution> &event)
|
||||
{
|
||||
const moveit_task_constructor::SolutionConstPtr& msg = event.getMessage();
|
||||
const moveit_task_constructor_msgs::SolutionConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
if (task_list_model_) task_list_model_->processSolutionMessage(id, *msg);
|
@ -44,9 +44,9 @@
|
||||
#include "job_queue.h"
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <ros/subscriber.h>
|
||||
#include <moveit_task_constructor/TaskDescription.h>
|
||||
#include <moveit_task_constructor/TaskStatistics.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/TaskDescription.h>
|
||||
#include <moveit_task_constructor_msgs/TaskStatistics.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#endif
|
||||
|
||||
namespace rviz
|
||||
@ -92,9 +92,9 @@ private Q_SLOTS:
|
||||
void onTasksRemoved(const QModelIndex& parent, int first, int last);
|
||||
void onTaskDataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight);
|
||||
|
||||
void taskDescriptionCB(const ros::MessageEvent<moveit_task_constructor::TaskDescription const>& event);
|
||||
void taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor::TaskStatistics> &event);
|
||||
void taskSolutionCB(const ros::MessageEvent<moveit_task_constructor::Solution const>& event);
|
||||
void taskDescriptionCB(const ros::MessageEvent<moveit_task_constructor_msgs::TaskDescription const>& event);
|
||||
void taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor_msgs::TaskStatistics> &event);
|
||||
void taskSolutionCB(const ros::MessageEvent<moveit_task_constructor_msgs::Solution const>& event);
|
||||
|
||||
protected:
|
||||
void updateTaskListModel();
|
@ -79,12 +79,17 @@ StageFactoryPtr getStageFactory()
|
||||
if (!factory.expired())
|
||||
return factory.lock();
|
||||
|
||||
StageFactoryPtr result(new StageFactory("moveit_task_constructor",
|
||||
"moveit::task_constructor::Stage"));
|
||||
// Hm. pluglinlib / ClassLoader cannot instantiate classes in implicitly loaded libs
|
||||
result->addBuiltInClass<moveit::task_constructor::SerialContainer>("Serial Container", "");
|
||||
factory = result; // remember for future uses
|
||||
return result;
|
||||
try {
|
||||
StageFactoryPtr result(new StageFactory("moveit_task_constructor_core",
|
||||
"moveit::task_constructor::Stage"));
|
||||
// Hm. pluglinlib / ClassLoader cannot instantiate classes in implicitly loaded libs
|
||||
result->addBuiltInClass<moveit::task_constructor::SerialContainer>("Serial Container", "");
|
||||
factory = result; // remember for future uses
|
||||
return result;
|
||||
} catch (const std::exception &e) {
|
||||
ROS_ERROR("Failed to initialize StageFactory");
|
||||
return StageFactoryPtr();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -292,7 +297,7 @@ bool TaskListModel::setData(const QModelIndex &index, const QVariant &value, int
|
||||
// process a task description message:
|
||||
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
|
||||
void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
const moveit_task_constructor::TaskDescription &msg)
|
||||
const moveit_task_constructor_msgs::TaskDescription &msg)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
|
||||
@ -330,7 +335,7 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
|
||||
// process a task statistics message
|
||||
void TaskListModel::processTaskStatisticsMessage(const std::string &id,
|
||||
const moveit_task_constructor::TaskStatistics &msg)
|
||||
const moveit_task_constructor_msgs::TaskStatistics &msg)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
|
||||
@ -346,7 +351,7 @@ void TaskListModel::processTaskStatisticsMessage(const std::string &id,
|
||||
}
|
||||
|
||||
void TaskListModel::processSolutionMessage(const std::string &id,
|
||||
const moveit_task_constructor::Solution &msg)
|
||||
const moveit_task_constructor_msgs::Solution &msg)
|
||||
{
|
||||
// TODO
|
||||
}
|
@ -37,12 +37,12 @@
|
||||
#pragma once
|
||||
|
||||
#include "pluginlib_factory.h"
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/TaskDescription.h>
|
||||
#include <moveit_task_constructor/TaskStatistics.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/TaskDescription.h>
|
||||
#include <moveit_task_constructor_msgs/TaskStatistics.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
|
||||
#include <QAbstractItemModel>
|
||||
#include <QTreeView>
|
||||
@ -112,11 +112,11 @@ public:
|
||||
bool removeRows(int row, int count, const QModelIndex &parent) override;
|
||||
|
||||
/// process an incoming task description message - only call in Qt's main loop
|
||||
void processTaskDescriptionMessage(const std::__cxx11::string &id, const moveit_task_constructor::TaskDescription &msg);
|
||||
void processTaskDescriptionMessage(const std::string &id, const moveit_task_constructor_msgs::TaskDescription &msg);
|
||||
/// process an incoming task description message - only call in Qt's main loop
|
||||
void processTaskStatisticsMessage(const std::__cxx11::string &id, const moveit_task_constructor::TaskStatistics &msg);
|
||||
void processTaskStatisticsMessage(const std::string &id, const moveit_task_constructor_msgs::TaskStatistics &msg);
|
||||
/// process an incoming solution message - only call in Qt's main loop
|
||||
void processSolutionMessage(const std::__cxx11::string &id, const moveit_task_constructor::Solution &msg);
|
||||
void processSolutionMessage(const std::string &id, const moveit_task_constructor_msgs::Solution &msg);
|
||||
|
||||
/// retrieve TaskModel in given row
|
||||
BaseTaskModel* getTask(int row) const;
|
@ -68,7 +68,7 @@ public:
|
||||
static TaskListModelCache& instance();
|
||||
|
||||
/// get TaskListModel for a TaskDisplay
|
||||
TaskListModelPtr getModel(const std::__cxx11::string &ns);
|
||||
TaskListModelPtr getModel(const std::string &ns);
|
||||
|
||||
/// get global TaskListModel instance used for panels
|
||||
TaskListModelPtr getGlobalModel();
|
@ -43,7 +43,7 @@
|
||||
#include "local_task_model.h"
|
||||
#include "factory_model.h"
|
||||
#include "pluginlib_factory.h"
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
#include <rviz/properties/property.h>
|
||||
#include <rviz/visualization_manager.h>
|
||||
@ -57,8 +57,10 @@ namespace moveit_rviz_plugin {
|
||||
rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) {
|
||||
static QPointer<rviz::PanelDockWidget> widget = nullptr;
|
||||
if (!widget && mgr) { // create widget
|
||||
QTreeView *view = new QTreeView();
|
||||
StageFactoryPtr factory = getStageFactory();
|
||||
if (!factory)
|
||||
return nullptr;
|
||||
QTreeView *view = new QTreeView();
|
||||
view->setModel(new FactoryModel(*factory, factory->mimeType(), view));
|
||||
view->expandAll();
|
||||
view->setHeaderHidden(true);
|
||||
@ -92,7 +94,9 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
||||
{
|
||||
setupUi(q_ptr);
|
||||
// init tasks view
|
||||
task_list_model->setStageFactory(getStageFactory());
|
||||
auto factory = getStageFactory();
|
||||
if (!factory) button_show_stage_dock_widget->setDisabled(true);
|
||||
task_list_model->setStageFactory(factory);
|
||||
tasks_view->setModel(task_list_model.get());
|
||||
|
||||
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
|
||||
@ -137,8 +141,9 @@ void TaskPanel::addTask()
|
||||
|
||||
void TaskPanel::showStageDockWidget()
|
||||
{
|
||||
Q_D(TaskPanel);
|
||||
rviz::PanelDockWidget *dock = getStageDockWidget(vis_manager_->getWindowManager());
|
||||
dock->show();
|
||||
if (dock) dock->show();
|
||||
}
|
||||
|
||||
void TaskPanel::removeTaskTreeRows()
|
14
visualization/motion_planning_tasks/test/CMakeLists.txt
Normal file
14
visualization/motion_planning_tasks/test/CMakeLists.txt
Normal file
@ -0,0 +1,14 @@
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-task_model
|
||||
motion_planning_tasks_rviz_plugin
|
||||
${catkin_LIBRARIES}
|
||||
gtest_main)
|
||||
endif()
|
@ -37,8 +37,8 @@
|
||||
#include <task_list_model.h>
|
||||
#include <local_task_model.h>
|
||||
#include <remote_task_model.h>
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
|
||||
#include <ros/init.h>
|
||||
#include <gtest/gtest.h>
|
||||
@ -54,13 +54,13 @@ protected:
|
||||
int num_inserts = 0;
|
||||
int num_updates = 0;
|
||||
|
||||
moveit_task_constructor::TaskDescription genMsg(const std::string &name) {
|
||||
moveit_task_constructor::TaskDescription t;
|
||||
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string &name) {
|
||||
moveit_task_constructor_msgs::TaskDescription t;
|
||||
t.id = name;
|
||||
uint id = 0, root_id;
|
||||
|
||||
moveit_task_constructor::StageDescription desc;
|
||||
moveit_task_constructor::StageStatistics stat;
|
||||
moveit_task_constructor_msgs::StageDescription desc;
|
||||
moveit_task_constructor_msgs::StageStatistics stat;
|
||||
desc.parent_id = stat.parent_id = id;
|
||||
desc.id = stat.id = root_id = ++id;
|
||||
desc.name = name;
|
@ -2,6 +2,6 @@
|
||||
<include file="$(find moveit_resources)/fanuc_moveit_config/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
<test pkg="moveit_task_constructor"
|
||||
type="moveit_task_constructor-test-task_model" test-name="test_task_model" />
|
||||
<test pkg="moveit_task_constructor_visualization"
|
||||
type="moveit_task_constructor_visualization-test-task_model" test-name="test_task_model" />
|
||||
</launch>
|
24
visualization/package.xml
Normal file
24
visualization/package.xml
Normal file
@ -0,0 +1,24 @@
|
||||
<package>
|
||||
<name>moveit_task_constructor_visualization</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Visualization tools for MoveIt Task Pipeline</description>
|
||||
|
||||
<license>BSD</license>
|
||||
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>moveit_task_constructor_msgs</build_depend>
|
||||
<build_depend>moveit_task_constructor_core</build_depend>
|
||||
<build_depend>rviz</build_depend>
|
||||
|
||||
<run_depend>moveit_task_constructor_msgs</run_depend>
|
||||
<run_depend>moveit_task_constructor_core</run_depend>
|
||||
<run_depend>rviz</run_depend>
|
||||
|
||||
<test_depend>rostest</test_depend>
|
||||
|
||||
<export>
|
||||
<rviz plugin="${prefix}/motion_planning_tasks_rviz_plugin_description.xml"/>
|
||||
</export>
|
||||
</package>
|
@ -1,9 +1,11 @@
|
||||
set(MOVEIT_LIB_NAME moveit_task_visualization_tools)
|
||||
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools)
|
||||
|
||||
set(HEADERS
|
||||
include/moveit_task_constructor/visualization_tools/display_solution.h
|
||||
include/moveit_task_constructor/visualization_tools/task_solution_visualization.h
|
||||
include/moveit_task_constructor/visualization_tools/task_solution_panel.h
|
||||
${PROJECT_INCLUDE}/display_solution.h
|
||||
${PROJECT_INCLUDE}/task_solution_visualization.h
|
||||
${PROJECT_INCLUDE}/task_solution_panel.h
|
||||
)
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME}
|
||||
|
@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
namespace moveit { namespace core {
|
||||
@ -48,7 +48,8 @@ public:
|
||||
return name(indexPair(index));
|
||||
}
|
||||
|
||||
void setFromMessage(const planning_scene::PlanningSceneConstPtr &parent, const moveit_task_constructor::Solution& msg);
|
||||
void setFromMessage(const planning_scene::PlanningSceneConstPtr &parent,
|
||||
const moveit_task_constructor_msgs::Solution& msg);
|
||||
};
|
||||
|
||||
}
|
@ -37,7 +37,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#include <QObject>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
@ -102,7 +102,7 @@ public:
|
||||
void onDisable();
|
||||
void setName(const QString& name);
|
||||
|
||||
void showTrajectory(const moveit_task_constructor::Solution& msg);
|
||||
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
|
||||
void dropTrajectory();
|
||||
|
||||
public Q_SLOTS:
|
@ -1,4 +1,4 @@
|
||||
#include <moveit_task_constructor/visualization_tools/display_solution.h>
|
||||
#include <moveit/visualization_tools/display_solution.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
|
||||
@ -39,7 +39,7 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const
|
||||
}
|
||||
|
||||
void DisplaySolution::setFromMessage(const planning_scene::PlanningSceneConstPtr& parent,
|
||||
const moveit_task_constructor::Solution &msg)
|
||||
const moveit_task_constructor_msgs::Solution &msg)
|
||||
{
|
||||
planning_scene::PlanningScenePtr ref_scene = parent->diff();
|
||||
|
||||
|
@ -34,7 +34,7 @@
|
||||
|
||||
/* Author: Yannick Jonetzko */
|
||||
|
||||
#include <moveit_task_constructor/visualization_tools/task_solution_panel.h>
|
||||
#include <moveit/visualization_tools/task_solution_panel.h>
|
||||
#include <QHBoxLayout>
|
||||
|
||||
namespace moveit_rviz_plugin
|
||||
|
@ -34,9 +34,9 @@
|
||||
|
||||
/* Author: Dave Coleman, Robert Haschke */
|
||||
|
||||
#include <moveit_task_constructor/visualization_tools/display_solution.h>
|
||||
#include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
|
||||
#include <moveit_task_constructor/visualization_tools/task_solution_panel.h>
|
||||
#include <moveit/visualization_tools/display_solution.h>
|
||||
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||
#include <moveit/visualization_tools/task_solution_panel.h>
|
||||
|
||||
#include <moveit/rviz_plugin_render_tools/planning_scene_render.h>
|
||||
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
|
||||
@ -478,7 +478,7 @@ void TaskSolutionVisualization::renderPlanningScene(const planning_scene::Planni
|
||||
scene_alpha_property_->getFloat());
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::Solution& msg)
|
||||
void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::Solution& msg)
|
||||
{
|
||||
// Error check
|
||||
if (!scene_)
|
||||
|
Loading…
Reference in New Issue
Block a user