47 #include <std_msgs/msg/color_rgba.hpp>
48 #include <tf2_eigen/tf2_eigen.hpp>
49 #include <visualization_msgs/msg/marker.hpp>
50 #include <visualization_msgs/msg/marker_array.hpp>
54 namespace visualization
59 const auto GREEN = [](
double a) {
60 std_msgs::msg::ColorRGBA color;
76 visualization_msgs::msg::MarkerArray
79 const std_msgs::msg::ColorRGBA& color = GREEN(1.0))
81 visualization_msgs::msg::MarkerArray markers_array;
84 visualization_msgs::msg::Marker sphere_marker;
85 sphere_marker.header.frame_id =
robot_trajectory.getRobotModel()->getModelFrame();
86 sphere_marker.ns =
"Path";
87 sphere_marker.type = visualization_msgs::msg::Marker::SPHERE;
88 sphere_marker.action = visualization_msgs::msg::Marker::ADD;
89 sphere_marker.lifetime = rclcpp::Duration(0, 0);
90 sphere_marker.scale.x = 0.01;
91 sphere_marker.scale.y = 0.01;
92 sphere_marker.scale.z = 0.01;
93 sphere_marker.color = color;
94 sphere_marker.frame_locked =
false;
97 for (std::size_t index = 0; index <
robot_trajectory.getWayPointCount(); index++)
99 const Eigen::Isometry3d& tip_pose =
robot_trajectory.getWayPoint(index).getGlobalLinkTransform(ee_parent_link);
100 sphere_marker.pose = tf2::toMsg(tip_pose);
101 sphere_marker.id = index;
103 markers_array.markers.push_back(sphere_marker);
106 return markers_array;
121 const std::shared_ptr<const planning_scene::PlanningScene>&
planning_scene,
124 assert(group !=
nullptr);
126 if (marker_publisher ==
nullptr)
128 return [](
int ,
double ,
const Eigen::MatrixXd& ) {
133 auto path_publisher = [marker_publisher, group,
135 int ,
double ,
const Eigen::MatrixXd& values) {
141 if (ee_parent_link !=
nullptr && !trajectory.empty())
143 marker_publisher->publish(createTrajectoryMarkerArray(trajectory, ee_parent_link, GREEN(0.5)));
147 return path_publisher;
161 const std::shared_ptr<const planning_scene::PlanningScene>&
planning_scene,
164 assert(group !=
nullptr);
166 if (marker_publisher ==
nullptr)
168 return [](
bool ,
int ,
double ,
const Eigen::MatrixXd& ) {
173 auto path_publisher =
175 bool success,
int ,
double ,
const Eigen::MatrixXd& values) {
183 if (ee_parent_link !=
nullptr && !trajectory.empty())
185 marker_publisher->publish(createTrajectoryMarkerArray(trajectory, ee_parent_link));
190 return path_publisher;
const moveit::core::LinkModel * getOnlyOneEndEffectorTip() const
Get one end effector tip, throwing an error if there ends up being more in the joint model group This...
A link from the robot. Contains the constant transform applied to the link and its geometry.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Maintain a sequence of waypoints and the time durations between these waypoints.
Helper functions for converting between MoveIt types and plain Eigen types.
This namespace includes the central class for representing planning contexts.
PostIterationFn getIterationPathPublisher(const rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr &marker_publisher, const std::shared_ptr< const planning_scene::PlanningScene > &planning_scene, const moveit::core::JointModelGroup *group)
Get post iteration function that publishes the EE path of the generated trajectory.
DoneFn getSuccessTrajectoryPublisher(const rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr &marker_publisher, const std::shared_ptr< const planning_scene::PlanningScene > &planning_scene, const moveit::core::JointModelGroup *group)
Get Done function that publishes the EE path of the generated trajectory.
std::function< void(bool success, int total_iterations, double final_cost, const Eigen::MatrixXd &values)> DoneFn
void fillRobotTrajectory(const Eigen::MatrixXd &trajectory_values, const moveit::core::RobotState &reference_state, robot_trajectory::RobotTrajectory &trajectory)
std::function< void(int iteration_number, double cost, const Eigen::MatrixXd &values)> PostIterationFn
A STOMP task definition that allows injecting custom functions for planning.