mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
DisplaySolution class
This commit is contained in:
parent
eb9753271a
commit
81126f5ef6
@ -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})
|
||||||
|
|||||||
@ -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);
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
67
visualization/visualization_tools/src/display_solution.cpp
Normal file
67
visualization/visualization_tools/src/display_solution.cpp
Normal 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
|
||||||
Loading…
Reference in New Issue
Block a user