From 81126f5ef688fcf835104eb29a2aa535be15f1b5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Nov 2017 20:13:58 +0100 Subject: [PATCH] DisplaySolution class --- .../visualization_tools/CMakeLists.txt | 24 ++++--- .../visualization_tools/display_solution.h | 54 +++++++++++++++ .../src/display_solution.cpp | 67 +++++++++++++++++++ 3 files changed, 134 insertions(+), 11 deletions(-) create mode 100644 visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/display_solution.h create mode 100644 visualization/visualization_tools/src/display_solution.cpp diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 4b4fcb80..d5456d67 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -1,27 +1,29 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools) set(HEADERS - include/moveit_task_constructor/visualization_tools/task_solution_visualization.h - include/moveit_task_constructor/visualization_tools/task_solution_panel.h + 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 ) add_library(${MOVEIT_LIB_NAME} - src/task_solution_visualization.cpp - src/task_solution_panel.cpp - ${HEADERS} + src/display_solution.cpp + src/task_solution_visualization.cpp + src/task_solution_panel.cpp + ${HEADERS} ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) target_link_libraries(${MOVEIT_LIB_NAME} - ${catkin_LIBRARIES} - ${OGRE_LIBRARIES} - ${QT_LIBRARIES} - ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${OGRE_LIBRARIES} + ${QT_LIBRARIES} + ${Boost_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) install(DIRECTORY include/ DESTINATION include) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/display_solution.h new file mode 100644 index 00000000..b606b00b --- /dev/null +++ b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/display_solution.h @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(RobotState) +} } +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene) +} +namespace robot_trajectory { +MOVEIT_CLASS_FORWARD(RobotTrajectory) +} + +namespace moveit_rviz_plugin { + +/** Class representing a task solution for display */ +class DisplaySolution +{ + size_t steps_; + std::vector scene_; + std::vector trajectory_; + std::vector name_; + +public: + size_t getWayPointCount() const { return steps_; } + bool empty() const { return steps_ == 0; } + + typedef std::pair IndexPair; + IndexPair indexPair(size_t index) const; + + float getWayPointDurationFromPrevious(const IndexPair& idx_pair) const; + float getWayPointDurationFromPrevious(size_t index) const { + if (index >= steps_) return 0.0; + return getWayPointDurationFromPrevious(indexPair(index)); + } + const moveit::core::RobotStatePtr& getWayPointPtr(const IndexPair& idx_pair) const; + const moveit::core::RobotStatePtr& getWayPointPtr(size_t index) const { + return getWayPointPtr(indexPair(index)); + } + const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const; + const planning_scene::PlanningSceneConstPtr& scene(size_t index) const { + return scene(indexPair(index)); + } + const std::string& name(const IndexPair& idx_pair) const; + const std::string& name(size_t index) const { + return name(indexPair(index)); + } + + void setFromMessage(const planning_scene::PlanningSceneConstPtr &parent, const moveit_task_constructor::Solution& msg); +}; + +} diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp new file mode 100644 index 00000000..2e7b2f3f --- /dev/null +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -0,0 +1,67 @@ +#include +#include +#include + +namespace moveit_rviz_plugin { + +std::pair DisplaySolution::indexPair(size_t index) const +{ + size_t part = 0; + for (const auto& t : trajectory_) { + if (index < t->getWayPointCount()) + break; + index -= t->getWayPointCount(); + ++part; + } + assert(part < trajectory_.size()); + assert(index < trajectory_[part]->getWayPointCount()); + return std::make_pair(part, index); +} + +float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair &idx_pair) const +{ + return trajectory_[idx_pair.first]->getWayPointDurationFromPrevious(idx_pair.second); +} + +const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair &idx_pair) const +{ + return trajectory_[idx_pair.first]->getWayPointPtr(idx_pair.second); +} + +const planning_scene::PlanningSceneConstPtr &DisplaySolution::scene(const IndexPair &idx_pair) const +{ + return scene_[idx_pair.first]; +} + +const std::string &DisplaySolution::name(const IndexPair &idx_pair) const +{ + return name_[idx_pair.first]; +} + +void DisplaySolution::setFromMessage(const planning_scene::PlanningSceneConstPtr& parent, + const moveit_task_constructor::Solution &msg) +{ + planning_scene::PlanningScenePtr ref_scene = parent->diff(); + + scene_.resize(msg.sub_trajectory.size()); + trajectory_.resize(msg.sub_trajectory.size()); + name_.resize(msg.sub_trajectory.size()); + + steps_ = 0; + size_t i = 0; + for (const auto& sub : msg.sub_trajectory) { + scene_[i] = ref_scene; + trajectory_[i].reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), "")); + trajectory_[i]->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory); + + name_[i] = sub.name; + steps_ += trajectory_[i]->getWayPointCount(); + + // create new reference scene based of diffs + ref_scene = ref_scene->diff(); + ref_scene->setPlanningSceneDiffMsg(sub.scene_diff); + ++i; + } +} + +} // namespace moveit_rviz_plugin