44 #include <boost/bind.hpp> 45 #include <boost/algorithm/string/erase.hpp> 46 #include <boost/foreach.hpp> 47 #include <boost/shared_ptr.hpp> 49 #include <OgreSceneNode.h> 50 #include <OgreSceneManager.h> 71 , messages_received_(0)
77 , angular_thres_(0.5
f)
83 QRegExp depth_filter(
"depth");
84 depth_filter.setCaseSensitivity(Qt::CaseInsensitive);
88 "List only topics with names that relate to depth and color images",
94 QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
95 "sensor_msgs::Image topic to subscribe to.",
107 QRegExp color_filter(
"color|rgb|bgr|gray|mono");
108 color_filter.setCaseSensitivity(Qt::CaseInsensitive);
112 QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
113 "sensor_msgs::Image topic to subscribe to.",
127 "Advanced: set the size of the incoming message queue. Increasing this " 128 "is useful if your incoming TF data is delayed significantly from your" 129 " image data, but it can greatly increase memory usage if the messages are big.",
136 "Automatically scale each point based on its depth value and the camera parameters.",
142 "Scaling factor to be applied to the auto size.",
150 "Keep points alive after they have been occluded by a closer point. Points are removed after a timeout or when the camera frame moves.",
157 "Amount of seconds before removing occluded points from the depth cloud",
203 if ( datatype == ros::message_traits::datatype<sensor_msgs::Image>() )
210 int index = topic.lastIndexOf(
"/");
213 ROS_WARN(
"DepthCloudDisplay::setTopic() Invalid topic name: %s",
214 topic.toStdString().c_str());
217 QString transport = topic.mid(index + 1);
218 QString base_topic = topic.mid(0, index);
256 if (use_occlusion_compensation)
309 if (!depthmap_topic.empty() && !depthmap_transport.empty()) {
321 if (!color_topic.empty() && !color_transport.empty()) {
378 boost::mutex::scoped_lock lock(
mutex_);
387 boost::mutex::scoped_lock lock(
mutex_);
407 sensor_msgs::ImageConstPtr rgb_msg)
414 std::ostringstream
s;
420 sensor_msgs::CameraInfo::ConstPtr cam_info;
426 if ( !cam_info || !depth_msg )
432 s << depth_msg->width <<
" x " << depth_msg->height;
438 s << rgb_msg->width <<
" x " << rgb_msg->height;
441 if (depth_msg->header.frame_id != rgb_msg->header.frame_id)
443 std::stringstream errorMsg;
444 errorMsg <<
"Depth image frame id [" << depth_msg->header.frame_id.c_str()
445 <<
"] doesn't match color image frame id [" << rgb_msg->header.frame_id.c_str() <<
"]";
452 float f = cam_info->K[0];
453 float bx = cam_info->binning_x > 0 ? cam_info->binning_x : 1.0;
460 if (use_occlusion_compensation)
463 Ogre::Quaternion orientation;
464 Ogre::Vector3 position;
471 QString(
"Failed to transform from frame [") + depth_msg->header.frame_id.c_str() + QString(
"] to frame [")
482 float angle_deg = angle.valueDegrees();
483 if (angle_deg >= 180.0
f)
485 if (angle_deg < -180.0
f)
506 if ( !cloud_msg.get() )
510 cloud_msg->header = depth_msg->header;
528 "image_transport::SubscriberPlugin");
537 std::string transport_name = boost::erase_last_copy(lookup_name,
"_sub");
538 transport_name = transport_name.substr(lookup_name.find(
'/') + 1);
567 property->clearOptions();
569 std::vector<std::string> choices;
571 choices.push_back(
"raw");
576 ros::master::V_TopicInfo::iterator it = topics.begin();
577 ros::master::V_TopicInfo::iterator end = topics.end();
578 for (; it != end; ++it)
586 const std::string& topic_name = ti.
name;
589 if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] ==
'/' 590 && topic_name.find(
'/', topic.size() + 1) == std::string::npos)
592 std::string transport_type = topic_name.substr(topic.size() + 1);
598 choices.push_back(transport_type);
603 for (
size_t i = 0; i < choices.size(); i++)
605 property->addOptionStd(choices[i]);
Ogre::Quaternion current_orientation_
boost::shared_ptr< SynchronizerDepthColor > sync_depth_color_
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
virtual void processMessage(sensor_msgs::Image::ConstPtr msg)
boost::scoped_ptr< image_transport::ImageTransport > rgb_it_
void scanForTransportSubscriberPlugins()
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.
virtual void setString(const QString &str)
virtual void expand()
Expand (show the children of) this Property.
virtual void updateAutoSizeFactor()
boost::shared_ptr< image_transport::SubscriberFilter > rgb_sub_
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
void fillTransportOptionList(EnumProperty *property)
Fill list of available and working transport options.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
virtual int getInt() const
Return the internal property value as an integer.
FloatProperty * point_world_size_property_
virtual void updateTopicFilter()
FloatProperty * auto_size_factor_property_
MultiLayerDepth * ml_depth_data_
ros::CallbackQueueInterface * getCallbackQueue()
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
message_filters::Synchronizer< SyncPolicyDepthColor > SynchronizerDepthColor
virtual float getFloat() const
void enableOcclusionCompensation(bool occlusion_compensation)
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
EnumProperty * xyz_transformer_property_
ROSCPP_DECL bool getTopics(V_TopicInfo &topics)
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
virtual void updateOcclusionTimeOut()
std::vector< TopicInfo > V_TopicInfo
Property specialized to enforce floating point max/min.
void enableFilter(bool enabled)
sensor_msgs::CameraInfo::ConstPtr cam_info_
virtual void onInitialize()
Override this function to do subclass-specific initialization.
Property specialized to provide max/min enforcement for integers.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
boost::shared_ptr< image_transport::SubscriberFilter > depthmap_sub_
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
virtual void updateTopic()
std::string getStdString()
Displays a point cloud of type sensor_msgs::PointCloud.
bool isEnabled() const
Return true if this Display is enabled, false if not.
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
std::set< std::string > transport_plugin_types_
bool setFloat(float new_value)
Float-typed "SLOT" version of setValue().
boost::scoped_ptr< image_transport::ImageTransport > depthmap_it_
virtual void reset()
Called to tell the display to clear its state.
void caminfoCallback(sensor_msgs::CameraInfo::ConstPtr msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
ApproximateTime< sensor_msgs::Image, sensor_msgs::Image > SyncPolicyDepthColor
boost::shared_ptr< message_filters::Subscriber< sensor_msgs::CameraInfo > > cam_info_sub_
virtual void reset()
Called to tell the display to clear its state.
EnumProperty * color_transformer_property_
void setCallbackQueue(CallbackQueueInterface *queue)
std::vector< std::string > getDeclaredClasses()
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
virtual const char * what() const
boost::mutex cam_info_mutex_
RosFilteredTopicProperty * color_topic_property_
std::string getCameraInfoTopic(const std::string &base_topic)
const std::string & getFixedFrame()
Return the current fixed frame name.
virtual void setTopic(const QString &topic, const QString &datatype)
Set the ROS topic to listen to for this display.
BoolProperty * use_occlusion_compensation_property_
PointCloudCommon * pointcloud_common_
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
sensor_msgs::PointCloud2Ptr generatePointCloudFromDepth(sensor_msgs::ImageConstPtr depth_msg, sensor_msgs::ImageConstPtr color_msg, sensor_msgs::CameraInfoConstPtr camera_info_msg)
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
IntProperty * queue_size_property_
ros::NodeHandle threaded_nh_
A NodeHandle whose CallbackQueue is run from a different thread than the GUI.
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
Constructor.
Ogre::Vector3 current_position_
void setShadowTimeOut(double time_out)
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
virtual QVariant getValue() const
Return the value of this Property as a QVariant. If the value has never been set, an invalid QVariant...
void setAutoSize(bool auto_size)
virtual void updateUseAutoSize()
virtual void updateUseOcclusionCompensation()
EnumProperty * depth_transport_property_
FloatProperty * occlusion_shadow_timeout_property_
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
EnumProperty * color_transport_property_
Property * topic_filter_property_
void hide()
Hide this Property in any PropertyTreeWidgets.
bool initialized() const
Returns true if the display has been initialized.
std::string getTopicStd() const
virtual ~DepthCloudDisplay()
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
bool setStdString(const std::string &std_str)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setString(const QString &str)
Set the value of this property to the given string. Does not force the value to be one of the list of...
uint32_t messages_received_
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
BoolProperty * use_auto_size_property_
void update(float wall_dt, float ros_dt)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > depthmap_tf_filter_
RosFilteredTopicProperty * depth_topic_property_