mirrors/rviz_navi_multi_goals_pub_plugin
1
0
Fork 0
mirror of https://github.com/autolaborcenter/rviz_navi_multi_goals_pub_plugin.git synced 2026-01-12 03:15:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

add pose marker number

Browse Source
This commit is contained in:
Ryoo
2020-09-10 16:36:13 +08:00
parent 1b0678a557
commit ba972acd8d
1 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/teleop_pad.cpp
View File

@@ -158,19 +158,26 @@ namespace navi_multi_goals_pub_rviz_plugin {
void TeleopPanel::markPose(const geometry_msgs::PoseStamped::ConstPtr &pose) {
if (ros::ok()) {
visualization_msgs::Marker arrow;
arrow.header.frame_id = pose->header.frame_id;
visualization_msgs::Marker number;
arrow.header.frame_id = number.header.frame_id = pose->header.frame_id;
arrow.ns = "navi_point_arrow";
arrow.action = visualization_msgs::Marker::ADD;
number.ns = "navi_point_number";
arrow.action = number.action = visualization_msgs::Marker::ADD;
arrow.type = visualization_msgs::Marker::ARROW;
arrow.pose = pose->pose;
number.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
arrow.pose = number.pose = pose->pose;
number.pose.position.z += 1.0;
arrow.scale.x = 1.0;
arrow.scale.y = 0.2;
arrow.color.r = 1.0f;
arrow.color.g = 250;
arrow.color.b = 205;
arrow.color.a = 1.0;
arrow.id = pose_array_.poses.size();
number.scale.z = 1.0;
arrow.color.r = number.color.r = 1.0f;
arrow.color.g = number.color.g = 0.98f;
arrow.color.b = number.color.b = 0.80f;
arrow.color.a = number.color.a = 1.0;
arrow.id = number.id = pose_array_.poses.size();
number.text = std::to_string(pose_array_.poses.size());
marker_pub_.publish(arrow);
marker_pub_.publish(number);
}
}