Add new package nxt_rviz_plugin for ROS Groovy

Author: dbworth

nxt/nxt_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+set(ROS_BUILD_TYPE RelWithDebInfo)
+rosbuild_init()
+
+find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
+include(${QT_USE_FILE})
+
+# Use Qt signals and slots to avoid defining "emit", "slots", etc.
+# because they can conflict with Boost signals, so define QT_NO_KEYWORDS here.
+add_definitions(-DQT_NO_KEYWORDS)
+
+# Specify which header files need to be run through "moc",
+# Qt's meta-object compiler.
+qt4_wrap_cpp(MOC_FILES
+  src/nxt_color_display.h
+  src/nxt_ultrasonic_display.h
+)
+
+set(SOURCE_FILES
+  src/nxt_color_display.cpp
+  src/nxt_color_visual.cpp
+  src/nxt_ultrasonic_display.cpp
+  src/nxt_ultrasonic_visual.cpp
+  ${MOC_FILES}
+)
+
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+# An rviz plugin is just a shared library, so here we declare the
+# library to be called PROJECT_NAME
+rosbuild_add_library(${PROJECT_NAME} ${SOURCE_FILES})
+
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
+

nxt/nxt_rviz_plugin/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

nxt/nxt_rviz_plugin/icons/classes/NXTColor.png

@@ -0,0 +1,26 @@
+<package>
+  <description brief="nxt_rviz_plugin">
+Plugins for visualizing data coming from the Lego NXT Brick in Rviz.
+Suitable for ROS Groovy and newer.
+  </description>
+  <author>David Butterworth</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org</url>
+
+  <depend package="roscpp"/>
+  <depend package="rviz"/>
+  <depend package="tf"/>
+  <depend package="geometry_msgs"/>
+  <depend package="nxt_msgs"/>
+
+  <export>
+      <rosdoc config="${prefix}/rosdoc.yaml"/>
+      <rviz plugin="${prefix}/plugin_description.xml"/>
+  </export>
+
+</package>
+
+
+
+

nxt/nxt_rviz_plugin/icons/classes/NXTUltrasonic.png

@@ -0,0 +1,20 @@
+<library path="lib/libnxt_rviz_plugin">
+
+  <class name="nxt_rviz_plugin/NXTColor"
+         type="nxt_rviz_plugin::NXTColorDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays data from a nxt_msgs::Color message as a cylinder of color.
+    </description>
+  </class>
+
+  <class name="nxt_rviz_plugin/NXTUltrasonic"
+         type="nxt_rviz_plugin::NXTUltrasonicDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays data from a nxt_msgs::Range message as a cone.
+    </description>
+  </class>
+
+</library>
+

nxt/nxt_rviz_plugin/manifest.xml

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+
+#
+# Script to publish dummy msgs
+# to test the NXT Rviz plugins.
+#
+
+import roslib; roslib.load_manifest('nxt_rviz_plugin')
+from nxt_msgs.msg import Color
+from nxt_msgs.msg import Range
+import rospy
+import tf
+
+color_topic = '/my_color_sensor'
+color_publisher = rospy.Publisher(color_topic, Color)
+color_values = list()
+color_values.append([0.0,0.0,0.0]) # black
+color_values.append([0.0,0.0,1.0]) # blue
+color_values.append([0.0,1.0,0.0]) # green
+color_values.append([1.0,1.0,0.0]) # yellow
+color_values.append([1.0,0.0,0.0]) # red
+color_values.append([1.0,1.0,1.0]) # white
+color_count = 0
+
+range_topic = '/my_range_sensor'
+range_publisher = rospy.Publisher(range_topic, Range)
+range_values = list()
+range_values.append(0.05)
+range_values.append(0.07)
+range_values.append(0.09)
+range_values.append(0.11)
+range_values.append(0.13)
+range_count = 0
+
+rospy.init_node('send_test_msgs')
+
+print "Publishing test Color messages to topic ", color_topic
+print "Publishing test Range messages to topic ", range_topic
+
+br = tf.TransformBroadcaster()
+
+rate = rospy.Rate(1)
+while not rospy.is_shutdown():
+
+    color_msg = Color()
+    color_msg.header.frame_id = "/color_sensor_frame"
+    color_msg.header.stamp = rospy.Time.now()
+    color_msg.r = color_values[color_count][0]
+    color_msg.g = color_values[color_count][1]
+    color_msg.b = color_values[color_count][2]
+    color_publisher.publish(color_msg)
+    color_count = color_count + 1
+    if color_count > (len(color_values)-1):
+        color_count = 0
+
+    range_msg = Range()
+    range_msg.header.frame_id = "/range_sensor_frame"
+    range_msg.header.stamp = rospy.Time.now()
+    range_msg.range = range_values[range_count]
+    range_msg.spread_angle = 0.2
+    range_publisher.publish(range_msg)
+    range_count = range_count + 1
+    if range_count > (len(range_values)-1):
+        range_count = 0
+
+    br.sendTransform((0, 0, 0),
+                     tf.transformations.quaternion_from_euler(0, 0, 0),
+                     rospy.Time.now(),
+                     "color_sensor_frame",
+                     "map")
+
+    br.sendTransform((0, 0, 0),
+                     tf.transformations.quaternion_from_euler(0, 0, 0),
+                     rospy.Time.now(),
+                     "range_sensor_frame",
+                     "map")
+
+    rate.sleep()
+

nxt/nxt_rviz_plugin/plugin_description.xml

@@ -0,0 +1,168 @@
+/*
+ * Copyright (c) 2016, David Butterworth
+ * All rights reserved.
+ *
+ * An Rviz plugin to display data from the 
+ * LEGO Mindstorms NXT 2.0 color sensor.
+ *
+ * Based on nxt_rviz_plugin & rviz_plugin_tutorials by Willow Garage.
+ * 
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/visualization_manager.h>
+#include <rviz/properties/float_property.h>
+#include <rviz/properties/int_property.h>
+#include <rviz/frame_manager.h>
+
+#include "nxt_color_visual.h"
+#include "nxt_color_display.h"
+
+namespace nxt_rviz_plugin
+{
+NXTColorDisplay::NXTColorDisplay()
+{
+  // Properties for this Display that the user
+  // can change from within Rviz:
+  cylinder_length_property_ = new rviz::FloatProperty("Length", 0.003,
+                                                      "Length of the cylinder visual.",
+                                                      this, SLOT(updateCylinderLength()));
+  alpha_property_ = new rviz::FloatProperty("Alpha", 0.5,
+                                            "0 is fully transparent, 1.0 is fully opaque.",
+                                            this, SLOT(updateAlpha()));
+  history_length_property_ = new rviz::IntProperty("History Length", 1,
+                                                   "Number of prior measurements to display.",
+                                                   this, SLOT(updateHistoryLength()));
+  history_length_property_->setMin(1);
+  history_length_property_->setMax(100000);
+}
+
+void NXTColorDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  updateHistoryLength();
+}
+
+NXTColorDisplay::~NXTColorDisplay()
+{
+}
+
+// Clear this display back to its initial state
+void NXTColorDisplay::reset()
+{
+  MFDClass::reset();
+  visuals_.clear();
+}
+
+// Qt slot: set the current color and alpha values for each visual.
+void NXTColorDisplay::updateAlpha()
+{
+  float alpha = alpha_property_->getFloat();
+
+  for(size_t i=0; i < visuals_.size(); i++)
+  {
+    visuals_[i]->setColor(color_.r, color_.g, color_.b, alpha);
+  }
+}
+
+// Qt slot: set the number of past visuals to show
+void NXTColorDisplay::updateHistoryLength()
+{
+  visuals_.rset_capacity(history_length_property_->getInt());
+}
+
+// Qt slot: set the length of the cylinder visual
+void NXTColorDisplay::updateCylinderLength()
+{
+  float length = cylinder_length_property_->getFloat();
+
+  for(size_t i=0; i < visuals_.size(); i++)
+  {
+    visuals_[i]->setScale(length);
+  }
+}
+
+// Callback to handle an incoming ROS message
+void NXTColorDisplay::processMessage(const nxt_msgs::Color::ConstPtr& msg)
+{
+  float length = cylinder_length_property_->getFloat();
+
+  // Get the transform from the fixed frame to the frame
+  // in the header of this msg
+  Ogre::Quaternion orientation;
+  Ogre::Vector3 position;
+  if( !context_->getFrameManager()->getTransform(msg->header.frame_id,
+                                                 msg->header.stamp,
+                                                 position,
+                                                 orientation))
+  {
+    ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
+              msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
+    return;
+  }
+
+  // We are keeping a circular buffer of visual pointers.
+  // Get the next ptr or make a new one.
+  boost::shared_ptr<NXTColorVisual> visual;
+  if(visuals_.full())
+  {
+    visual = visuals_.front();
+  }
+  else
+  {
+    visual.reset(new NXTColorVisual(context_->getSceneManager(), scene_node_));
+  }
+
+  // Update the pose of the Ogre::SceneNode
+  visual->setFramePosition(position);
+  visual->setFrameOrientation(orientation);
+
+  // Set the pose and size of the cylinder visual
+  visual->setScale(length);
+
+  // Store the RGB color value from the msg
+  // and set the color of the cylinder visual
+  float alpha = alpha_property_->getFloat();
+  color_.r = msg->r;
+  color_.g = msg->g;
+  color_.b = msg->b;
+  visual->setColor(color_.r, color_.g, color_.b, alpha);
+
+  // Add visual to end of the circular buffer
+  visuals_.push_back(visual);
+}
+
+} // end namespace nxt_rviz_plugin
+
+// Tell pluginlib about this class
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(nxt_rviz_plugin::NXTColorDisplay,rviz::Display)
+