brice rebsamen's profile - activity

This is a follow up on some discussion going on here: http://answers.ros.org/question/2826/simulating-sensor-data-problem-with-rosbag-and Since the discussion has drifted away from the original topic I am posting a new question...

I am writing a simulation that simulates a moving vehicle and outputs ground truth position (as an Odometry message on topic "ground_truth" and as a TF: ground_truth -> base_link), and simulated sensor data with a controllable amount of noise. I want to use it to test my localization algorithms (EKF).

The simulator can run both in real time and in bag mode. In real time mode, it publishes the messages on topics, and everything is fine. In bag mode, it runs as fast as possible and writes the messages directly into a bag file, using the rosbag C++ API.

The only thing that is missing is how to write TF messages in the rosbag. I tried writing them to the bag on topic /tf but I got the following problems:

1) Writing tf/StamedTransform (same as what I broadcast in real time mode) I get the following error at compilation: ‘const class tf::StampedTransform’ has no member named ‘__getDataType’

2) Writing geometry_msgs::TransformStamped I convert using tf::transformStampedTFToMsg. No compilation errors. But in rviz, after setting the Fixed Frame to /ground_truth, I get the following warning: "no tf data. Fixed Frame [/ground_truth] does not exist." And the drop down menu to choose the fixed frame is empty.

So I do I proceed to store TF messages / transforms into the bag?

is there a version of getParamCached() in rospy?

In roscpp: "ros::NodeHandle::getParamCached() and ros::param::getCached() provide local caching of parameter data. Using these versions informs the Parameter Server that this node would like to be notified when the parameter is changed, and prevents the node from having to re-lookup the value with the parameter server on subsequent calls."

If not, what would it take to implement it?

I am calling rospy.get_param() in a tight loop. I get some exceptions from httplib (CannotSendRequest and ResponseNotReady). Is it because I am calling get_param() too often? What over exceptions should I look for?

assume 2 callbacks:

void cb1(const MsgType &);
void cb2(const MsgType::ConstPtr &);

In the case of cb2, each time a message is received a new object is created, i.e. memory allocation occurs. Well unless it's a nodelet, in which case the memory was allocated by the sender... but I'm not talking about that...

What about cb1? Is it the same object each time, i.e. memory does not need to be allocated if the message is the same size? That could save some time when dealing with large messages...

EDIT: Since nobody understood my question, I'll try to reformulate. Actually I'd love to look at the actual code that does the deserialization and calls the callback function. Can somebody points it to me?

Here is 2 possible implementations of the message queue. This is pseudo code... but which concept is closer to the actual implementation?

// implementation that does not allocate the memory for each new message
class MessageQueue {
    queue<MsgType::SerializedType> serialized_msgs_;
    MsgType deserialized_msg_; //this object is allocated only once and reused

    void process() {
        deserialized_msg_.deserialize(serialized_msgs_.front());
        serialized_msgs_.pop();
        notify_callback( deserialized_msg_ ); //pass by const reference
    }
};

// another implementation that allocates new memory each time
class MessageQueue {
    queue<MsgType::SerializedType> serialized_msgs_;

    void process() {
        // allocate a new object each time
        MsgTypePtr deserialized_msg(new MsgType);
        deserialized_msg->deserialize(serialized_msgs_.front());
        serialized_msgs_.pop();
        notify_callback(deserialized_msg);
    }
};

In the first implementation, if we are dealing with a message that is always the same size, then no allocation is required at all which makes the code super efficient.

do I have to provide them myself:

namespace std_msgs {
bool operator== (const Header &h1, const Header &h2)
{
  return h1.frame_id==h2.frame_id && h1.stamp==h2.stamp && h1.seq==h2.seq;
}
}

namespace geometry_msgs {
bool operator== (const Point &p1, const Point &p2)
{
  return p1.x==p2.x && p1.y==p2.y && p1.z==p2.z;
}
bool operator== (const Quaternion &q1, const Quaternion &q2)
{
  return q1.x==q2.x && q1.y==q2.y && q1.z==q2.z && q1.w==q2.w;
}
bool operator== (const Pose &p1, const Pose &p2)
{
  return p1.position==p2.position && p1.orientation==p2.orientation;
}
}

etc...

that's seems dumb. I checked the generated header files and no comparison operator is provided. The default constructor initializes things to 0, there is a serialization mechanism, etc. but not comparison operator. Am I missing a point? Is there a good reason to not provide these operators?

I went through the code carefully, I can't figure out where are the 42 header bytes.

According to the doc for the 32E, the data packet should start with a 42 bytes header, followed by 1200 bytes of data (block + encoder + points), terminated by 6 status bytes.

In the pcap reading code, those are discarded (input.cc line 269). I cannot find any trace of it in the InputSocket code. Can anyone explain?

With rosbuild, I used to download data when building and place them in the src folder. I could them find them at runtime with ros::package::getPath().

This is usually data that is too large to be hosted in the repository, like large binary configuration files. They are needed by some of our nodes, and they usually load them when they start. As such they must be accessible at run time.

Now in catkin, I am wondering where to download them and how to retrieve them at runtime. I created a cmake function based on download_checkmd5.py that creates a new target (download_extra_data). Each package CMakeLists can declare some data to be downloaded, which happens when I make that target. What's the best strategy?

I can download them to the source folder and retrieve them with ros::package::getPath(), but that goes against the principle of not touching the source folder. Also, this would probably not work if I were to distribute my package in binary form (as the ros packages are), so it does not seem like a good solution.

I can download them to the build folder, but then how to retrieve them at runtime?

I believe the best would be to download them to the devel space, right? How can I do that? Then how to retrieve them at runtime?

For the moment, as a temporary solution, I am downloading to a path specified by an environment variable, but I am looking for a cleaner solution.

In C++, is there a way to list parameters on the parameter server, optionally those in a particular namespace? Alternatively, can I use a dictionary/map?

I want to put a mapping name/number in a yaml file loaded by roslaunch, and my node should be able to list the names and retrieve the associated number. rosparam list can do that, and associated with grep I can retrieve only the parameters I want. can I do that in C++ directly. the parameter server c++ API does not seem to provide that, or is it hidden? Can it be added in future releases?

I tried using a dictionary but it was expended in a list of parameters instead of being a dictionary data structure, i.e. those 2 yaml files yielded the same result

channel_numbers: {applanix: 0, radar: 1, trimble: 5, velodyne: 10}

and

channel_numbers:
  applanix: 0
  radar: 1
  trimble: 5
  velodyne: 10

I would like something like:

ros::NodeHandle nh("~");
vector<string> parameters = nh.list_parameters("channel_numbers");
// and then for each parameter in parameters get its value...

I have a wet package called velodyne, which provides a roscpp node called conversion. It builds alright, I can see it in the devel space ([WS]/devel/lib/velodyne/conversion), it has the executable bit set, but rosrun cannot find because it looks for it in the source folder.

I've had this problem a lot recently. Usually, after resourcing my setup several times, rebuilding several times, and other random fiddling, I finally can launch the node. I would like to know what's the issue and how to get rid of it...

my catkin workspace is located in /data/driving_repos/catkin_ws in my bashrc I source /data/driving_repos/catkin_ws/devel/setup.bash

 $ rosrun velodyne conversion 
[rosrun] Couldn't find executable named conversion below /data/driving_repos/catkin_ws/src/rdr_v2/packages/perception/velodyne
[rosrun] Found the following, but they're either not files,
[rosrun] or not executable:
[rosrun]   /data/driving_repos/catkin_ws/src/rdr_v2/packages/perception/velodyne/src/conversion

Indeed, /data/driving_repos/catkin_ws/src/rdr_v2/packages/perception/velodyne/src/conversion is a folder!

Interestingly, after typing 'rosrun velodyne ' and using tab completion, it lists all the executables in my velodyne package alright...

Also, uninstalling ros-hydro-velodyne solves my problem. I checked the source code of rosrun and it calls

catkin_find --without-underlays --libexec --share velodyne

which returns /opt/ros/hydro/share/velodyne !

It's strange however that it returns that path... shouldn't the packages in my workspace have priority?

Hi

I started playing around with an Arduino Uno and rosserial. I am trying to control 2 stepper motors (really a motor controller via pulse/dir signal, using the AccelStepper library), a PWM signal, and few infrequent IOs. The pulse generation is a bit intensive because I need to generate a fairly large number of steps per seconds (about 1000 steps per seconds per motor).

I tested it, motor by motor and it seems to be alright. However, when I connect the arduino to the joystick (via ROS / rosserial python_node), it sends quite a large number of messages (I counted up to 160 messages per seconds). The motor start spinning when I touch the joystick, but after a while the arduino seems to hang.

My guess is that I am sending to many messages, thus stalling the communication channel and overloading the arduino with message decoding. Could this be?

Based on this assumption, I started to write a filter: on the PC side, a node receives the messages destined to the Arduino (i.e. set the position of motor 1 to x), and only send the latest commands, aggregated in one message, every xx ms (currently 25 ms). This will definitely reduce the amount of communication on the serial line and the amount of decoding on the arduino's side, but as a drawback, it will reduce the responsiveness of my hardware: motors will be updated less frequently (I think it should be OK for my application though).

I will try to report whether this is a successful strategy. Right now I am having a problem with rosserial and custom messages...

Hello

I wrote a simple simulator that generates sensor data (ideal values plus some noise). My goal is to test my localization algorithms (EKF). It seems to work, but I am having problems with recording a bag file.

Here is how it works: given an acceleration profile, I integrate it to obtain ground truth (position, orientation, velocity, etc.). I then generate encoder and IMU data from that ground truth. Once all the data has been generated, I publish it sequentially on respective topics, together with clock messages.

Here some part of the code:

ros::init(argc, argv, "sensorSimulator");
bool realtime = false; //value obtained from command line or parameters...
float period = 0.01; //period of the simulation

// Generate the data
// (...)
vector<StampedData *> data = profile.generateData();

// Create publishers on the following topics
// sensor_msgs::Imu on /ms/imu/data
// rosgraph_msgs::Clock on /clock
// lowlevel::Encoders (custom) on /encoders
// fmutil::SimpleOdo (custom) on /ground_truth_linear
// nav_msgs::Odometry on /ground_truth
// tf::TransformBroadcaster
// (...)


// Publish the data
for( unsigned i=0; i<data.size() && ros::ok(); i++ )
{
    rosgraph_msgs::Clock clockmsg;
    clockmsg.clock = ros::Time(data[i]->time);
    clockPub.publish( clockmsg );

    if( data[i]->type == ground_truth_t ) {
        // publish the ground truth odometry and TF
        // (...)
        if( realtime )
            ros::WallDuration(period).sleep();
    }
    else if( data[i]->type == imu_t ) {
        //publish IMU data
        // (...)
    }
    else if( data[i]->type == encoders_t ) {
        // publish encoders data
        // (...)
    }
}

This works: when I start rviz (with use_sim_time=true) and run my code with the real time option on, I can see the simulated vehicle moving on the desired trajectory. I can record all the data in a bag file as well.

However, if I want to generate the data and record it in a bag it does not work: roscore rosbag record -a -O sensorSimulation rosrun mypack sensorSimulation Once this is done, I CTRL-C the rosbag process. I can then inspect the content of the bag with rosbag info sensorSimulation.bag. Data is missing: only a few of the topics are recorded, and duration is wrong (too short).

I have an hypothesis to explain what's happening. I am publishing too fast on the topics, and some data gets lost. So I am wondering: is there a way to monitor the status of the publishers and associate mechanisms, and slow down the publishing process accordingly? Btw, I should mention that my publishers are created with a message queue of size 0 (infinite).

Hi

I have topics that publish quaternions (e.g. Pose, Odometry, etc.). This is not very human-friendly, e.g. when doing a rostopic echo, or a rxplot.

I am dealing with a car, and I can assume a 2D model most of the time, so yaw=2*acos(w). I know this can be done from the code, so what I'm doing for now is to publish in parallel a message that gives rpy, that I can use for debugging. But it would be nice if there was some kind of filter that could be applied on live or recorded data to do the transformation.

Turns out that ros::init in roscpp sets the global logger object to be a ROSOutAppender which publishes to /rosout.

I've been reading the rosconsole code and I can't find any publisher to /rosout. rosconsole dependencies are only rostime, cpp_common, rostime and console_bridge. Can someone explain exactly how those work?

I see that rosconsole is a wrapper around log4cxx. It handles the initialization, provides vargs interface, formatting and the logging macros. I am confused about what console bridge is supposed to do. There is also the rosout package that dumps the messages on the /rosout topic to a file (which seems redundant to what rosconsole is doing...)

But I can't find a publisher from rosconsole to /rosout