Add Sphere marker

Add Sphere marker

Author: Magic-wei

README.md

@@ -25,6 +25,7 @@ Currently support:
 - Line List
 - Cylinder
 - Cube
+- Sphere
 - Arrow
 - Text
 - Frame (Axes-type Pose)

include/ros_viz_tools/ros_viz_tools.h

@@ -78,6 +78,13 @@ class RosVizTools {
                           const ColorRGBA &color,
                           const std::string &frame_id);
 
+    static Marker newSphere(const double &scale,
+                            const geometry_msgs::Pose &pose,
+                            const std::string &ns,
+                            const int32_t &id,
+                            const ColorRGBA &color,
+                            const std::string &frame_id);
+
     static Marker newArrow(const geometry_msgs::Vector3 &scale,
                            const geometry_msgs::Pose &pose,
                            const std::string &ns,

src/demo_node.cpp

@@ -115,6 +115,15 @@ int main( int argc, char** argv )
         visualization_msgs::Marker marker_cube = RosVizTools::newCube(1.0, pose , ns, 0, ros_viz_tools::WHITE, frame_id);
         markers.append(marker_cube);
 
+        // Cube
+        ns = "sphere";
+        pose.position.x = -3.0;
+        pose.position.y = -3.0;
+        pose.position.z = -3.0;
+        pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 45 * M_PI / 180, 45 * M_PI / 180));
+        visualization_msgs::Marker marker_sphere = RosVizTools::newSphere(0.5, pose , ns, 0, ros_viz_tools::RED, frame_id);
+        markers.append(marker_sphere);
+
         // Arrow
         ns = "arrow";
         scale.x = 1.0;

src/ros_viz_tools.cpp

@@ -124,6 +124,19 @@ Marker RosVizTools::newCube(const double &scale,
     return newMaker(vec_scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::CUBE);
 }
 
+Marker RosVizTools::newSphere(const double &scale,
+                              const geometry_msgs::Pose &pose,
+                              const std::string &ns,
+                              const int32_t &id,
+                              const ColorRGBA &color,
+                              const std::string &frame_id) {
+    geometry_msgs::Vector3 vec_scale;
+    vec_scale.x = scale;
+    vec_scale.y = scale;
+    vec_scale.z = scale;
+    return newMaker(vec_scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::SPHERE);
+}
+
 Marker RosVizTools::newArrow(const geometry_msgs::Vector3 &scale,
                              const geometry_msgs::Pose &pose,
                              const std::string &ns,