Hello,

I have a yaml file that I use to get the extrinsic parameters of a stereo camera calibration to broadcast them to TF. I load them in the launch file and use them in a cpp file. The problem is if I don't load them first within the terminal with rosparam load pathtofile, I always get an error each parameter cannot be loaded. Here are the yaml, launch and cpp file:

yaml:

translation_vector:
  rows: 1
  cols: 3
  data: [-6.718487289238215e-05, 0.010357273240538437, 0.0005853021648665484]
rotation_matrix:
  rows: 1
  cols: 3
  data: [-0.9999990189116211, -0.0013801575387492157, -0.00023945973262821368, 0.0013761198212543803, -0.9998692197431468, 0.016113649632171025, -0.0002616677910394789, 0.01611330429797217, 0.9998701380452202] 

launch:

<launch>
    <node pkg="get_extrinsic" type="get_extrinsic" name="get_extrinsic" output="screen">
        <rosparam file="/home/ariel/.ros/extrinsic.yaml" command="load" ns="extrinsic_parameters"/>
    </node>
</launch>

cpp:

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <tf/transform_broadcaster.h>
#include <ros/param.h>
#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <cv_bridge/cv_bridge.h>

using namespace std;
using namespace cv;

int main(int argc, char** argv)
{
    std::vector<double> T, R;
    double cols, rows, R_cols, R_rows;
    int i, j;
    cv::Mat rot_vec = Mat::zeros(1,3,CV_64F), rot_mat = Mat::zeros(3,3,CV_64F);

    ros::init(argc, argv, "get_extrinsic");
    ros::NodeHandle node;

    if(!node.getParam("/get_extrinsic/extrinsic_parameters/rotation_matrix/cols", cols))
    {
        ROS_ERROR_STREAM("Translation vector (cols) could not be read.");
        return 0;
    }
    if(!node.getParam("translation_vector/rows", rows))
    {
        ROS_ERROR_STREAM("Translation vector (rows) could not be read.");
        return 0;
    }
    T.reserve(cols*rows);

    if(!node.getParam("rotation_matrix/cols", cols))
    {
        ROS_ERROR_STREAM("Rotation matrix (cols) could not be read.");
        return 0;
    }
    if(!node.getParam("rotation_matrix/rows", rows))
    {
        ROS_ERROR_STREAM("Rotation matrix (rows) could not be read.");
        return 0;
    }
    R.reserve(cols*rows);

    if(!node.getParam("translation_vector/data", T))
    {
        ROS_ERROR_STREAM("Translation vector could not be read.");
        return 0;
    }

    if(!node.getParam("rotation_matrix/data", R))
    {
        ROS_ERROR_STREAM("Rotation matrix could not be read.");
        return 0;
    }

    for(i=0; i<3; i++)
    {
        for(j=0; j<3; j++)
            rot_mat.at<double>(i,j) = R[i*3+j];
    }

    std::cout << "Rotation Matrix:"<<endl;
    for(i=0; i<3; i++)
    {
        for(j=0; j<3; j++)
            std::cout<< rot_mat.at<double>(i,j) << " ";
        std::cout << endl;
    }
    std::cout << endl;

    std::cout << "Rodrigues: "<<endl;
    Rodrigues(rot_mat, rot_vec);

    for(i=0; i<3; i++)
        std::cout << rot_vec.at<double>(1,i) << " ";
    std::cout << endl;

    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion q;

    transform.setOrigin(tf::Vector3(T[0], T[1], T[2]));
    q.setRPY(rot_vec.at<double>(1,0), rot_vec.at<double>(1,1), rot_vec.at<double>(1,2));
    transform.setRotation(q);

    while(ros::ok())
    {
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/stereo/left", "/stereo/right"));
        ros::Duration(0.5).sleep();
    }

    ros::spin();
    return 0;
};

If I just do roslaunch I always get the error, but loading the yaml file through the terminal before launching the node it works..... Isn't rosparam file="/home/ariel/.ros/extrinsic.yaml" command="load" ns="extrinsic_parameters" in the launch file supposed to do the same?

Thanks for the help.

/get_extrinsic/extrinsic_parameters/rotation_matrix/cols
/get_extrinsic/extrinsic_parameters/rotation_matrix/data
/get_extrinsic/extrinsic_parameters/rotation_matrix/rows
/get_extrinsic/extrinsic_parameters/translation_vector/cols
/get_extrinsic/extrinsic_parameters/translation_vector/data
/get_extrinsic/extrinsic_parameters/translation_vector/rows

if(!node.getParam("/get_extrinsic/extrinsic_parameters/rotation_matrix/cols", cols))

if(!node.getParam("rotation_matrix/cols", cols))