Ask Your Question
0

Can't load rosparams in cpp [closed]

asked 2016-01-26 09:17:21 -0500

Ariel gravatar image

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 path_to_file, 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 ... (more)

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Ariel
close date 2016-01-27 02:20:29.918600

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-01-26 10:34:01 -0500

updated 2016-01-26 10:34:38 -0500

I think you are just having a namespace issue. By placing the <rosparam> tag inside of the get_extrinsic node tag you are telling all of the parameters to load up inside of the get_extrinsic namespace. For example, running your launch file then the rosparam list command should return

/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

In your code, you look up the first one using an absolute name that agrees with the above names.... see the following line:

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

All of the rest of your params you lookup using relative naming like this:

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

With your current launch file, that will resolve to /rotation_matrix/cols not /get_extrinsic/extrinsic_parameters/rotation_matrix/cols. Using a private nodehandle would be a good way to fix this. Also check out the Names page and the roscpp/Overview/NodeHandles page.

edit flag offensive delete link more

Comments

I get that exact rosparam list and that's why I use an absolute name for the first one (just to check) and I must have forgotten to catkin_make because I changed all the parameters to absolute and they are loaded correctly now. Thanks for the help!. I'll check the private nodehandle as well.

Ariel gravatar imageAriel ( 2016-01-27 02:20:17 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2016-01-26 09:17:21 -0500

Seen: 614 times

Last updated: Jan 26 '16