#12 now have the librviz node publishing when roslaunched with a came…

…ra info, it is currently all black so try adding a grid next.
lucasw · Sep 10, 2016 · 0684aed · 0684aed
1 parent 1a85fcb
commit 0684aed
Show file tree

Hide file tree

Showing 4 changed files with 55 additions and 3 deletions.
diff --git a/librviz_camera/launch/demo.launch b/librviz_camera/launch/demo.launch
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="use_rviz" default="true" />
+
+
+  <group ns="camera1">
+  <node pkg="librviz_camera" type="librviz_camera" name="librviz_camera"
+      output="screen" />
+  <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
+      args="0.1 0.1 -0.5 0 0 0 1 map camera1 10" />
+  <node name="camera_info" pkg="rostopic" type="rostopic"
+      args="pub camera_info sensor_msgs/CameraInfo
+     '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'camera1'},
+      height: 480, width: 640, distortion_model: 'plumb_bob',
+      D: [0],
+      K: [500.0, 0.0, 320, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0],
+      R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
+      P: [500.0, 0.0, 320, 0.0, 0.0, 500, 240, 0.0, 0.0, 0.0, 1.0, 0.0],
+      binning_x: 0, binning_y: 0,
+      roi: {x_offset: 0, y_offset: 0, height: 480, width: 640, do_rectify: false}}' -r 2"
+      output="screen"/>
+  </group>
+
+  <!--
+  <group ns="camera2">
+  <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
+      args="0.1 -0.1 -2.5 0.01 0 0.02 1 map camera2 10" />
+  <node name="camera_info" pkg="rostopic" type="rostopic"
+      args="pub camera_info sensor_msgs/CameraInfo
+     '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'camera2'},
+      height: 720, width: 1280, distortion_model: 'plumb_bob',
+      D: [0],
+      K: [300.0, 0.0, 640, 0.0, 300.0, 360.0, 0.0, 0.0, 1.0],
+      R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
+      P: [300.0, 0.0, 640, 0.0, 0.0, 300, 360, 0.0, 0.0, 0.0, 1.0, 0.0],
+      binning_x: 0, binning_y: 0,
+      roi: {x_offset: 0, y_offset: 0, height: 720, width: 1280, do_rectify: false}}' -r 2"
+      output="screen"/>
+  </group>
+  -->
+
+</launch>
diff --git a/librviz_camera/src/panel.cpp b/librviz_camera/src/panel.cpp
@@ -23,6 +23,8 @@ Panel::Panel(QWidget* parent)
   const bool enabled = true;
   camera_ = manager_->createDisplay("rviz_camera_stream/CameraPub", "rviz camera", enabled);
   ROS_ASSERT(camera_ != NULL);
+  camera_->subProp("Image Topic")->setValue("image_raw");
+  camera_->subProp("Camera Info Topic")->setValue("camera_info");
 }
 
 Panel::~Panel()

diff --git a/rviz_camera_stream/launch/demo.launch b/rviz_camera_stream/launch/demo.launch
@@ -5,6 +5,7 @@
   <!-- launch rviz with config/rviz_camera_stream.rviz -->
   <node pkg="rviz" type="rviz" name="rviz"
       args="-d $(find rviz_camera_stream)/config/rviz_camera_stream.rviz"
+      output="screen"
       if="$(arg use_rviz)" />
 
   <group ns="camera1">

diff --git a/rviz_camera_stream/src/camera_display.cpp b/rviz_camera_stream/src/camera_display.cpp
@@ -340,6 +340,7 @@ void CameraPub::onEnable()
 {
   subscribe();
   render_texture_->setActive(true);
+  ROS_INFO_STREAM("enable " << int(video_publisher_->is_active()));
 }
 
 void CameraPub::onDisable()
@@ -352,19 +353,23 @@ void CameraPub::onDisable()
 void CameraPub::subscribe()
 {
   if (!isEnabled())
+  {
     return;
+  }
 
   std::string topic_name = topic_property_->getTopicStd();
   if (topic_name.empty())
   {
-    setStatus(StatusProperty::Error, "Output Topic", "No topic set");
+    setStatus(StatusProperty::Error, "Image Topic", "No topic set");
+    ROS_WARN_STREAM("No image topic set");
     return;
   }
 
   std::string error;
   if (!ros::names::validate(topic_name, error))
   {
-    setStatus(StatusProperty::Error, "Output Topic", QString(error.c_str()));
+    setStatus(StatusProperty::Error, "Image Topic", QString(error.c_str()));
+    ROS_WARN_STREAM(error.c_str());
     return;
   }
 
@@ -373,6 +378,7 @@ void CameraPub::subscribe()
   if (caminfo_topic.empty())
   {
     setStatus(StatusProperty::Error, "Camera Info", "No topic set");
+    ROS_WARN_STREAM("no camera info topic set");
     return;
   }
 
@@ -392,7 +398,8 @@ void CameraPub::subscribe()
   }
 
   video_publisher_->advertise(topic_name);
-  setStatus(StatusProperty::Ok, "Output Topic", "Topic set");
+  setStatus(StatusProperty::Ok, "Image Topic", "Topic set");
+  ROS_INFO_STREAM("fully enabled");
 }
 
 void CameraPub::unsubscribe()