Merge pull request #33 from neufieldrobotics/feature/tf_prefix

will prefix tf_prefix to imagemsg and camerainfo regular frame_id upo…

Author: mithundiddi

include/spinnaker_sdk_camera_driver/capture.h

@@ -104,7 +104,7 @@ namespace acquisition {
         float exposure_time_;
         double target_grey_value_;
         // int decimation_;
-
+        string tf_prefix_;        
         int soft_framerate_; // Software (ROS) frame rate
 
         int MASTER_CAM_;
@@ -128,7 +128,7 @@ namespace acquisition {
         // grid view related variables
         bool GRID_CREATED_;
         Mat grid_;
-
+    
         // ros variables
         ros::NodeHandle nh_;
         ros::NodeHandle nh_pvt_;

launch/acquisition.launch

@@ -21,11 +21,13 @@
   <arg name="max_rate_save"     default="false"	doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
   <arg name="config_file"       default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
 
-  <!-- nodelet params-->
+  <!-- nodelet manager params-->
   <arg name="nodelet_manager_name" default="vision_nodelet_manager" doc="name of the nodelet manager, comes handy when launching multiple nodelets from different launch files" />
   <arg name="start_nodelet_manager" default="true" doc="If set to True(default), creates a nodelet manager with $(arg nodelet_manager_name).
   If False, the acquisition/capture_nodelet waits for the nodelet_manager name $(arg nodelet_manager_name)" />
 
+  <!-- Capture nodelet params-->
+  <arg name="tf_prefix" default="" />
    <!-- start the nodelet manager if $(arg start_nodelet_manager) is true-->
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" output="screen" if="$(arg start_nodelet_manager)" />
 
@@ -51,8 +53,9 @@
     <param name="time"              value="$(arg time)" />
     <param name="utstamps"          value="$(arg utstamps)" />
     <param name="to_ros"            value="$(arg to_ros)"/>
-    <param name="max_rate_save"     value="$(arg max_rate_save)"/>
-
+    <param name="max_rate_save"     value="$(arg max_rate_save)" />
+    <param name="tf_prefix"         value="$(arg tf_prefix)" />
+    
   </node>
 
 </launch>

launch/acquisition_node.launch

@@ -21,8 +21,11 @@
   <arg name="utstamps"          default="false"	doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
   <arg name="max_rate_save"     default="false"	doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
   <arg name="config_file"       default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
-
+  
+  <!-- acquisiton node params-->
+  <arg name="tf_prefix" default="" />
   <!-- load the acquisition node -->
+  
   <node pkg="spinnaker_sdk_camera_driver" type="acquisition_node" name="acquisition_node" output="$(arg output)" args="" respawn="false" >
 
     <!-- load the acquisition node parameters file.  Note any parameters provided in this file will 
@@ -45,6 +48,7 @@
     <param name="utstamps"          value="$(arg utstamps)" />
     <param name="to_ros"            value="$(arg to_ros)"/>
     <param name="max_rate_save"     value="$(arg max_rate_save)"/>
+    <param name="tf_prefix"         value="$(arg tf_prefix)" />
 
   </node>    
 

launch/multiple_nodelet_example.launch

@@ -2,6 +2,8 @@
     <!-- args namespacing  -->
     <arg name="robot_ns"                    default="$(env ROBOT_NAME)" />
     <arg name="vision_nm" default="vision_nodelet_manager_uas" doc="name of the nodelet manager" />
+    <arg name="camera_tf_prefix" default="" />
+
 
     <group ns="$(arg robot_ns)">    
         <!-- launch nodelet manager -->
@@ -10,6 +12,7 @@
         <include file="$(find spinnaker_sdk_camera_driver)/launch/acquisition.launch" >
             <arg name="nodelet_manager_name" value="$(arg vision_nm)" />
             <arg name="start_nodelet_manager" value="false" />
+            <arg name="tf_prefix" value="$(arg camera_tf_prefix)" />
         </include>
 
         <!-- launch apriltag_ros nodelet -->

launch/nodelet_subscriber_example.launch

@@ -21,14 +21,15 @@
   <arg name="max_rate_save"     default="false" doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
   <arg name="config_file"       default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
 
-  <!-- nodelet params-->
+  <!-- nodelet manager params-->
   <arg name="nodelet_manager_name" default="vision_nodelet_manager" doc="name of the nodelet manager, comes handy when launching multiple nodelets from different launch files" />
   <arg name="start_nodelet_manager" default="false" doc="If set to True, creates a nodelet manager with $(arg nodelet_manager_name).
   If False, the acquisition/capture_nodelet waits for the nodelet_manager name $(arg nodelet_manager_name)" />
 
+  <!-- Capture nodelet params-->
+  <arg name="tf_prefix" default="" />
 
   <!-- start the nodelet manager -->
-
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" output="screen" />
 
   <!-- load the acquisition nodelet -->
@@ -54,6 +55,7 @@
     <param name="utstamps"          value="$(arg utstamps)" />
     <param name="to_ros"            value="$(arg to_ros)"/>
     <param name="max_rate_save"     value="$(arg max_rate_save)"/>
+    <param name="tf_prefix"         value="$(arg tf_prefix)" />
 
   </node>
 

src/capture.cpp

@@ -478,6 +478,15 @@ void acquisition::Capture::read_parameters() {
             }else ROS_INFO("    Frames will be recorded until user termination");
         }else ROS_WARN("    'frames' Parameter not set, using defult behavior, frames will be recorded until user termination");
     }
+    
+    if (nh_pvt_.hasParam("tf_prefix")){
+        nh_pvt_.param<std::string>("tf_prefix", tf_prefix_, "");
+        ROS_INFO_STREAM("  tf_prefix set to: "<<tf_prefix_);
+    }
+    else ROS_WARN("  'tf_prefix' Parameter not set, using default behavior tf_prefix=" " ");
+
+    
+
 
     bool intrinsics_list_provided = false;
     XmlRpc::XmlRpcValue intrinsics_list;
@@ -766,9 +775,13 @@ void acquisition::Capture::export_to_ROS() {
     double t = ros::Time::now().toSec();
     std_msgs::Header img_msg_header;
     img_msg_header.stamp = mesg.header.stamp;
+    string frame_id_prefix;
+    if (tf_prefix_.compare("") != 0)
+        frame_id_prefix = tf_prefix_ +"/";
+    else frame_id_prefix="";
 
     for (unsigned int i = 0; i < numCameras_; i++) {
-        img_msg_header.frame_id = "cam_"+to_string(i)+"_optical_frame";
+        img_msg_header.frame_id = frame_id_prefix + "cam_"+to_string(i)+"_optical_frame";
 
         if(color_)
             img_msgs[i]=cv_bridge::CvImage(img_msg_header, "bgr8", frames_[i]).toImageMsg();
@@ -777,6 +790,7 @@ void acquisition::Capture::export_to_ROS() {
 
         if (PUBLISH_CAM_INFO_){
             cam_info_msgs[i]->header.stamp = mesg.header.stamp;
+            cam_info_msgs[i]->header.frame_id = frame_id_prefix + "cam_"+to_string(i)+"_optical_frame";
         }
         camera_image_pubs[i].publish(img_msgs[i],cam_info_msgs[i]);
 /*