DisplaySolution class

This commit is contained in:
Robert Haschke 2017-11-10 20:13:58 +01:00
parent eb9753271a
commit 81126f5ef6
3 changed files with 134 additions and 11 deletions

View File

@ -1,27 +1,29 @@
set(MOVEIT_LIB_NAME moveit_task_visualization_tools) set(MOVEIT_LIB_NAME moveit_task_visualization_tools)
set(HEADERS set(HEADERS
include/moveit_task_constructor/visualization_tools/task_solution_visualization.h include/moveit_task_constructor/visualization_tools/display_solution.h
include/moveit_task_constructor/visualization_tools/task_solution_panel.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} add_library(${MOVEIT_LIB_NAME}
src/task_solution_visualization.cpp src/display_solution.cpp
src/task_solution_panel.cpp src/task_solution_visualization.cpp
${HEADERS} src/task_solution_panel.cpp
${HEADERS}
) )
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
target_link_libraries(${MOVEIT_LIB_NAME} target_link_libraries(${MOVEIT_LIB_NAME}
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${OGRE_LIBRARIES} ${OGRE_LIBRARIES}
${QT_LIBRARIES} ${QT_LIBRARIES}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
) )
target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include)
install(DIRECTORY include/ DESTINATION include) install(DIRECTORY include/ DESTINATION include)
install(TARGETS ${MOVEIT_LIB_NAME} install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

View File

@ -0,0 +1,54 @@
#pragma once
#include <moveit_task_constructor/Solution.h>
#include <moveit/macros/class_forward.h>
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<planning_scene::PlanningSceneConstPtr> scene_;
std::vector<robot_trajectory::RobotTrajectoryPtr> trajectory_;
std::vector<std::string> name_;
public:
size_t getWayPointCount() const { return steps_; }
bool empty() const { return steps_ == 0; }
typedef std::pair<size_t, size_t> 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);
};
}

View File

@ -0,0 +1,67 @@
#include <moveit_task_constructor/visualization_tools/display_solution.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
namespace moveit_rviz_plugin {
std::pair<size_t, size_t> 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