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,11 +1,13 @@
|
||||
set(MOVEIT_LIB_NAME moveit_task_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
|
||||
)
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME}
|
||||
src/display_solution.cpp
|
||||
src/task_solution_visualization.cpp
|
||||
src/task_solution_panel.cpp
|
||||
${HEADERS}
|
||||
|
||||
@ -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