mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
marker tools: allow vector of LinkModel*
This commit is contained in:
parent
b1fac6eed0
commit
7e66c24822
@ -10,6 +10,7 @@ MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||
}
|
||||
namespace moveit { namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotState)
|
||||
MOVEIT_CLASS_FORWARD(LinkModel)
|
||||
} }
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
@ -28,12 +29,18 @@ void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr &scen
|
||||
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<std::string> &link_names = {});
|
||||
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<const moveit::core::LinkModel*> &link_models);
|
||||
|
||||
/** generate marker msgs to visualize robot's visual geometry, calling the given callback for each of them
|
||||
* link_names: set of links to include (or all if empty) */
|
||||
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<std::string> &link_names = {});
|
||||
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<const moveit::core::LinkModel*> &link_models);
|
||||
|
||||
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
|
||||
* calls generateMarkersForRobot() and generateMarkersForObjects() */
|
||||
|
||||
@ -70,6 +70,15 @@ template <class T> const std::string& materialName(const T& element);
|
||||
template <> const std::string& materialName(const urdf::Visual& element) { return element.material_name; }
|
||||
template <> const std::string& materialName(const urdf::Collision& element) { static std::string empty; return empty; }
|
||||
|
||||
std::vector<std::string> linkNames(const std::vector<const moveit::core::LinkModel*>& link_models)
|
||||
{
|
||||
std::vector<std::string> names;
|
||||
names.reserve(link_models.size());
|
||||
for (const moveit::core::LinkModel* link : link_models)
|
||||
names.push_back(link->getName());
|
||||
return names;
|
||||
}
|
||||
|
||||
/** generate marker msgs to visualize the robot state, calling the given callback for each of them
|
||||
* link_names: set of links to include (or all if empty) */
|
||||
template <class T> // with T = urdf::Visual or urdf::Collision
|
||||
@ -101,9 +110,11 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
|
||||
}
|
||||
};
|
||||
|
||||
// either we have an array of collision/visual elements
|
||||
for(const auto& element : elements_vector<T>(*link))
|
||||
element_handler(element);
|
||||
|
||||
// or there is a single such element
|
||||
if (!valid_found)
|
||||
element_handler(element<T>(*link));
|
||||
}
|
||||
@ -114,12 +125,22 @@ void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
|
||||
const std::vector<std::string> &link_names) {
|
||||
generateMarkers<urdf::Collision>(robot_state, callback, link_names);
|
||||
}
|
||||
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback &callback,
|
||||
const std::vector<const moveit::core::LinkModel*> &link_models) {
|
||||
generateMarkers<urdf::Collision>(robot_state, callback, linkNames(link_models));
|
||||
}
|
||||
|
||||
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<std::string> &link_names) {
|
||||
generateMarkers<urdf::Visual>(robot_state, callback, link_names);
|
||||
}
|
||||
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback &callback,
|
||||
const std::vector<const moveit::core::LinkModel*> &link_models) {
|
||||
generateMarkers<urdf::Visual>(robot_state, callback, linkNames(link_models));
|
||||
}
|
||||
|
||||
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
|
||||
* calls generateMarkersForRobot() and generateMarkersForObjects() */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user