How to get list element from rosparam parameter from the cmd? - ros

I loaded this yaml file:
num_boxes: 1
boxes: [[x: 0.349, y: 0.213, z: 0.117]]
with rosparam load my_config.yaml then I can do rosparam get boxes and get:
- - {x: 0.349}
- {y: 0.213}
- {z: 0.117}
But how can I access only the first list or elements in the second list? I tried boxes[0], boxes(0) and boxes{0} but nothing worked.

This is an old question, but I'm answering it because it's still useful to put an answer here.
Using the example given, boxes is a list of lists (YAML syntax). From the rosparam command line tool, we can't process rosparam get /boxes any further (excepting grep and regex). In common use, we can access the Parameter Server in C++ / Python.
# Python
>>> import rospy
>>> boxes = rospy.get_param("/boxes"); boxes
[[{'x': 0.349}, {'y': 0.213}, {'z': 0.117}]]
# Boxes is a python list/array of list of dicts
>>> boxes[0]
[{'x': 0.349}, {'y': 0.213}, {'z': 0.117}]
>>> boxes[0][0]
{'x': 0.349}
// C++
#include <vector>
#include <ros/ros.h>
// yaml_list: [1, 2]
ros::NodeHandle *nh;
std::vector<int> yaml_list;
double x = 0.0;
int main(int argc, char** argv){
ros::init(argc, argv, "get_params_node");
nh = new ros::NodeHandle("");
nh->getParam("/yaml_list", yaml_list);
x = yaml_list[0];
// ...
return 0;
}
To have deeper data structures, such as vectors of vectors of maps, you have to use the xmlrpcpp (.h/.cpp) interface.
// C++
#include <vector>
#include <map>
#include <string>
#include <ros/ros.h>
#include <xmlrpcpp/XmlRpcValue.h> // catkin component
ros::NodeHandle *nh;
XmlRpc::XmlRpcValue boxes; // std::vector<std::vector<std::map<std::string,double>>>
double x = 0.0;
int i = 0;
double point[3] = {0};
int main(int argc, char** argv){
ros::init(argc, argv, "get_params_node");
nh = new ros::NodeHandle("");
nh->getParam("/boxes", boxes);
if(boxes.getType() == XmlRpc::XmlRpcValue::Type::TypeArray && boxes.size() > 0){
// boxes[0] is a 'TypeArray' aka vector
if(boxes[0].getType() == XmlRpc::XmlRpcValue::Type::TypeArray && boxes[0].size() > 0){
// boxes[0][0] is a 'TypeStruct' aka map
if(boxes[0][0].getType() == XmlRpc::XmlRpcValue::Type::TypeStruct && boxes[0][0].hasMember("x")){
x = double(boxes[0][0]["x"]);
for(XmlRpc::XmlRpcValue::iterator it = boxes[0][0].begin(); it != boxes[0][0].end(); ++it){
point[i++] = double(*it);
}
}
}
}
// ...
return 0;
}
In standard use, the <rosparam> XML tag keeps the same YAML or rosparam command line syntax.

Related

how to remove floor surface in the pointcloud2 data with the pcl_ros

first, thanks for reading.
now i am trying to do remove floor in the point cloud data,
this is a code that i wrote to remove the floor point cloud.
#include <iostream>
#include <cmath>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <velodyne_pointcloud/point_types.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <set>
#include <pcl/io/pcd_io.h>
#include <boost/format.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring) (float, time, time)
)
ros::Publisher pub1;
using PointXYZIRT = VelodynePointXYZIRT;
void help (const sensor_msgs::PointCloud2ConstPtr& scan)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<VelodynePointXYZIRT>::Ptr cloud(new pcl::PointCloud<VelodynePointXYZIRT>());
pcl::fromROSMsg (*scan, *cloud);
pcl::ModelCoefficients coefficients;
pcl::PointIndices inliers;
// Create the segmentation object
pcl::SACSegmentation< pcl::PointCloud<PointXYZIRT>> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud.makeShared ());
seg.segment (inliers, coefficients);
// Publish the model coefficients
pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(coefficients, ros_coefficients);
pub1.publish (ros_coefficients);
}
int main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("input", 1, help);
// Create a ROS publisher for the output point cloud
//#pub1 = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
pub1 = nh.advertise<pcl_msgs::ModelCoefficients> ("output", 1);
// Spin
ros::spin ();
}
as i have to put the ring data in the code, so i made the struct for defining the velodyne lidar,
but when i catkin_make in the ros,
this error comes,
error: ‘pcl::PointCloud<VelodynePointXYZIRT>::Ptr {aka class boost::shared_ptr<pcl::PointCloud<VelodynePointXYZIRT> >}’ has no member named ‘makeShared’
seg.setInputCloud (cloud.makeShared ());
is there a method that i visualize the data?
my reference for making the code is this site,https://adioshun.gitbooks.io/pcl-tutorial/content/part-1/part01-chapter05/part01-chapter05-practice.html
and my previous code to transform the lidar data, i used these code to make my own.....
#include <iostream>
#include <cmath>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <velodyne_pointcloud/point_types.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <set>
#include <pcl/io/pcd_io.h>
#include <boost/format.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#define PI 3.14159265359
using namespace std;
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring) (float, time, time)
)
ros::Publisher pub1;
float theta_r = 45* M_PI/ 180; // 라디안 각도로 회전 (180도 회전)
using PointXYZIRT = VelodynePointXYZIRT;
void input(const sensor_msgs::PointCloud2ConstPtr& scan)
{
// Msg to pointcloud
pcl::PointCloud<VelodynePointXYZIRT>::Ptr cloud(new pcl::PointCloud<VelodynePointXYZIRT>());
pcl::fromROSMsg(*scan,*cloud); // ros msg 에서 pcl cloud 데이터로 변환
//회전변환행렬
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
transform_1 (0,0) = std::cos (theta_r);
transform_1 (0,2) = std::sin(theta_r);
transform_1 (2,0) = -sin (theta_r);
transform_1 (2,2) = std::cos (theta_r);
// (row, column)
// Executing the transformation
pcl::PointCloud<VelodynePointXYZIRT>::Ptr transformed_cloud (new pcl::PointCloud<PointXYZIRT>());
pcl::transformPointCloud (*cloud, *transformed_cloud, transform_1);
pcl::PCLPointCloud2 cloud_p;
pcl::toPCLPointCloud2(*transformed_cloud, cloud_p);
sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(cloud_p, output);
output.header.frame_id = "velodyne";
pub1.publish(output);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "input");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("vlp202", 100, input);
pub1 = nh.advertise<sensor_msgs::PointCloud2> ("vlp203", 100);
ros::spin();
}
//if you have to make your own type of custum point type in the pcl, you can see this one to see how to code it !!
// also the ring data that you have to use is from the lio_sam, image projection.cpp
so for the abstract,
i want to remove floor point cloud data in recorded bag,
so i made some code to remove floor data
The "cloud" object is already a shared pointer. Remove makeShared call:
seg.setInputCloud (cloud.makeShared ());
to
seg.setInputCloud (cloud);
For visualization you could publish over a topic the result as ros message and visualize it with rviz.

How to run OpenACC + OpenCV with PGI in Windows?

I tried to run the code below:
#include <chrono>
#include <omp.h>
#include <vector>
#include <iostream>
#include <algorithm>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
using namespace std;
int main()
{
std::string imgstr = "input.bmp";
cv::Mat imgmat = cv::imread(imgstr, cv::IMREAD_GRAYSCALE);
imgmat.convertTo(imgmat, CV_64FC1, 1.0 / 255.0);
double st = omp_get_wtime();
int i, j;
double res = 0.0;
#pragma acc parallel loop
for (i =0; i <= 10; i++) {
for (j =0; j <=10; j++) {
res =res+ imgmat.at<double>(i, j);
}
}
double runtime = omp_get_wtime() - st;
printf("\n total: %f s\n", runtime);
}
with pgcc -fast -ta=nvidia,managed -Minfo=accel -o runEx runEx.c -lopencv_legacy -lopencv_highgui -lopencv_core && ./runEx in PGI, but I get an error saying
can't find include file opencv2/opencv.hpp
However, the code above compiles and runs successfully in Visual Studio without using OpenACC
can anyone help?
Most likely you need to include the path to header files using the "-I/path/to/openvc/include" flag on the command line. You'll also want to include the library path (via the "-L/path/to/opencv/lib") so it can find the libraries.
Though given "pgcc" is a C compiler, you'll have additional issues given you're using C++ header files and C++ constructs in your code. Current PGI releases do not support C++ on Windows.

How can I store a big matrix within the .cc file?

I am currently working on a Computer Vision / Machine Learning project for university. Sadly, they only allow us to upload one single file and restrict the computation time too much. Hence I need to compute the matrices on my machine and store them in the same file as the code (22500 rows, 1 col and 100 rows + 22500 col and 100 rows + 1 col). I already found a way to export the data (link), but I'm not sure how to initialize the matrix.
What I've tried
#include <opencv/cv.h>
#include <opencv/highgui.h>
int main(int argc, char const *argv[])
{
float data[10] = {1,2,3,4,5,7,8,9,10,11};
cv::Mat A;
// Something is wrong with this line
A = cv::Mat(1, 10, cv::CV_32FC1, data);
return 0;
}
When I compile it, I get:
main.cc: In function ‘int main(int, const char**)’:
main.cc:10:16: error: expected primary-expression before ‘(’ token
A = cv::Mat(1, 10, cv::CV_32FC1, data);
^
In file included from /usr/include/opencv2/core/core_c.h:47:0,
from /usr/include/opencv/cv.h:63,
from main.cc:1:
main.cc:10:28: error: expected unqualified-id before ‘(’ token
A = cv::Mat(1, 10, cv::CV_32FC1, data);
^
Second try
#include <opencv/cv.h>
#include <opencv/highgui.h>
int main(int argc, char const *argv[])
{
float dataHeaderMat1[10] = {1,2,3,4,5,7,8,9,10,11};
cv::Mat matrix1;
// Something is wrong with this line
cv::cvInitMatHeader( &matrix1, 10, 1, CV_64FC1, dataHeaderMat1);
return 0;
}
gives
main.cc:10:5: error: ‘cvInitMatHeader’ is not a member of ‘cv’
cv::cvInitMatHeader( &matrix1, 10, 1, CV_64FC1, dataHeaderMat1);
^
The following works to declare and initialize a matrix:
#include <opencv/cv.h>
#include <opencv/highgui.h>
int main(int argc, char const *argv[])
{
float data[10] = {1,2,3,4,5,7,8,9,10,11};
cv::Mat A;
// Something is wrong with this line
A = cv::Mat(1, 10, CV_32FC1, data);
return 0;
}
However, I'm not too sure if this is the best way for big arrays.
You can try to save image to header file, like this:
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
// uncomment for test
//#include "image.h"
int main(int argc, char **argv)
{
// This part creates header file from image.
Mat img=imread("D:\\ImagesForTest\\lena.jpg");
int w=img.cols;
int h=img.rows;
int channels=img.channels();
ofstream os("image.h");
os << "int rows=" << h << ";" << endl;
os << "int cols=" << w << ";" << endl;
os << "unsigned char d[]={" << endl;
for(int i=0;i<h;++i)
{
for(int j=0;j<w;++j)
{
if(i!=(w-1) || j!=(h-1))
{
Vec3b b=img.at<Vec3b>(i,j);
os << format("0x%02x,",b[0]);
os << format("0x%02x,",b[1]);
os << format("0x%02x,",b[2]);
}
}
}
Vec3b b=img.at<Vec3b>(w-1,h-1);
os << format("0x%02x,",b[0]);
os << format("0x%02x,",b[1]);
os << format("0x%02x",b[2]);
os << endl << "};" << endl;
os << "Mat I=Mat(rows,cols,CV_8UC3,d);" << endl;
os.close();
// To test uncomment commented part of code and comment uncommented.
// uncomment for test
/*
namedWindow("I");
imshow("I",I);
waitKey();
return 0;
*/
}
But be careful, not all IDEs likes such large files.

Can you improve this solution to interfacing OpenCV 2.4+ to Zxing 1D barcode reader

I didn't find this solution on the net, had to figure it myself. So, for benefit of others, I'm posing this as a "question":
Can you improve my working interface of OpenCV 2.4+ to the C++ version of Zxing 2.2 1D barcode reader?
Here's my working but perhaps improvable implementation below:
/**
* Gary Bradski, Reading 1D barcodes
* License BSD, (c) 2013
*
* Working example of how to call zxing using OpenCV 2.4+ cv::Mat
*
* Calling example, this one for 128 barcodes:
*
* Code128Reader cr; //Instantiate a zxing barcode reader, int this case for 128 barcodes,
* // but you can use any of their 1D or multi readers here
* ... by magic, I find, rectify and islotate a barcode into cv::Mat barcodeImage
* decode_image(&cr, barcodeImage); //Decode the isolated rectified barcode or fail
*
*/
#include <string>
#include <fstream>
#include <iostream>
#include <vector>
using namespace cv;
using namespace std;
//////////////ZXING BARCODE READER//////////////////////////////////////////
#include <zxing/LuminanceSource.h>
#include <zxing/MultiFormatReader.h>
#include <zxing/oned/OneDReader.h>
#include <zxing/oned/EAN8Reader.h>
#include <zxing/oned/EAN13Reader.h>
#include <zxing/oned/Code128Reader.h>
#include <zxing/datamatrix/DataMatrixReader.h>
#include <zxing/qrcode/QRCodeReader.h>
#include <zxing/aztec/AztecReader.h>
#include <zxing/common/GlobalHistogramBinarizer.h>
#include <zxing/Exception.h>
using namespace zxing;
using namespace oned;
using namespace datamatrix;
using namespace qrcode;
using namespace aztec;
class OpenCVBitmapSource : public LuminanceSource
{
private:
cv::Mat m_pImage;
public:
OpenCVBitmapSource(cv::Mat &image)
: LuminanceSource(image.cols, image.rows)
{
m_pImage = image.clone();
}
~OpenCVBitmapSource(){}
int getWidth() const { return m_pImage.cols; }
int getHeight() const { return m_pImage.rows; }
ArrayRef<char> getRow(int y, ArrayRef<char> row) const //See Zxing Array.h for ArrayRef def
{
int width_ = getWidth();
if (!row)
row = ArrayRef<char>(width_);
const char *p = m_pImage.ptr<char>(y);
for(int x = 0; x<width_; ++x, ++p)
row[x] = *p;
return row;
}
ArrayRef<char> getMatrix() const
{
int width_ = getWidth();
int height_ = getHeight();
ArrayRef<char> matrix = ArrayRef<char>(width_*height_);
for (int y = 0; y < height_; ++y)
{
const char *p = m_pImage.ptr<char>(y);
int yoffset = y*width_;
for(int x = 0; x < width_; ++x, ++p)
{
matrix[yoffset + x] = *p;
}
}
return matrix;
}
/*
// The following methods are not supported by this demo (the DataMatrix Reader doesn't call these methods)
bool isCropSupported() const { return false; }
Ref<LuminanceSource> crop(int left, int top, int width, int height) {}
bool isRotateSupported() const { return false; }
Ref<LuminanceSource> rotateCounterClockwise() {}
*/
};
void decode_image(Reader *reader, cv::Mat &image)
{
try
{
Ref<OpenCVBitmapSource> source(new OpenCVBitmapSource(image));
Ref<Binarizer> binarizer(new GlobalHistogramBinarizer(source));
Ref<BinaryBitmap> bitmap(new BinaryBitmap(binarizer));
Ref<Result> result(reader->decode(bitmap, DecodeHints(DecodeHints::TRYHARDER_HINT)));//+DecodeHints::DEFAULT_HINT)));
cout << result->getText()->getText() << endl;
//Export the read barcode here
}
catch (zxing::Exception& e)
{
//Export your failure to read the code here
cerr << "Error: " << e.what() << endl;
}
}
Forgot to attribute what I started with. There is an out of date (will not compile) implementation using IplImages here:
http://www.idealsoftware.com/opensource/scan-1d-2d-barcodes-webcam-zxing-opencv-visual-c.html
This updates that solution so that it works with Zxing 2.2 and OpenCV 2.1+
I think you can avoid the matrix copy by replacing
Ref<OpenCVBitmapSource> source(new OpenCVBitmapSource(image));
With
Ref<LuminanceSource> source(new GreyscaleLuminanceSource(image.data, image.step, image.rows, 0, 0, image.cols, image.rows));

lua5.2's error: multiple Lua VMs detected

I use 5.2 for learning recently, what I want to try like this:
Step 1, build a c module for lua:
#include "lua.h"
#include "lauxlib.h"
#include "lualib.h"
#include <stdlib.h>
static int add(lua_State *L) {
int x = luaL_checkint(L, -2);
int y = luaL_checkint(L, -1);
lua_pushinteger(L, x + y);
return 1;
}
static const struct luaL_Reg reg_lib[] = {
{"add", add}
};
int luaopen_tool(lua_State *L) {
luaL_newlib(L, reg_lib);
lua_setglobal(L, "tool");
return 0;
}
I compile and link it with liblua.a, and I'm sure it works well in lua script like "require("tool") tool.add(1, 2)"
Step 2, I write another C program that wants to require my c module in step 1 like this:
#include "lua.h"
#include "lauxlib.h"
#include "lualib.h"
#include <stdlib.h>
int main(int argc, char* const argv[]) {
lua_State *L = luaL_newstate();
luaL_requiref(L, "base", luaopen_base, 1);
luaL_requiref(L, "package", luaopen_package, 1);
lua_getglobal(L, "require");
if (!lua_isfunction(L, -1)) {
printf("require not found\n");
return 2;
}
lua_pushstring(L, "tool");
if (lua_pcall(L, 1, 1, 0) != LUA_OK) {
printf("require_fail=%s\n", lua_tostring(L, -1));
return 3;
}
lua_getfield(L, -1, "add");
lua_pushinteger(L, 2);
lua_pushinteger(L, 3);
lua_pcall(L, 2, 1, 0);
int n = luaL_checkint(L, -1);
printf("n=%d\n", n);
return 0;
}
I also compile & link with liblua.a, but error occurs when I run it:
"require_fail=multiple Lua VMs detected"
Someone's blog said that in lua5.2, you should link c module and c host program both dynamicly, but not staticly.
is there someone that has the same problem, or is there somehing wrong in my code, thanks.
NOTICE:
the problem has been solved by compile main program with -Wl,-E, thanks a lot for all your help ^^.
Don't link your C module with liblua.a when you create a .so from it. For examples, see my page of Lua libraries: http://www.tecgraf.puc-rio.br/~lhf/ftp/lua/ . You can link liblua.a statically into your main program but you have to export its symbols by adding -Wl,-E at link time. That's how the Lua interpreter is built in Linux.

Resources