ros-simulation/stage_ros

Package which contains ROS specific hooks and tools for the Stage simulator.

stage_ros's Issues

Is is expected for ground truth position and odometry to be VERY different?

Hello

I have been using stageros to simulate a robot running navigation 2d application [https://github.com/skasperski/navigation_2d].
I am trying to compare the odometry and ground truth pose that is published by stage every time the WorldCallback function is called. I have observed that they can differ by a lot, the stats are as below :

odom_msg.pose.pose.position.x - gt.getOrigin().x() : [mean: 3.94237572, median: 4.14029, stdev: 5.71846084, max: 13.5125]
odom_msg.pose.pose.position.y - gt.getOrigin().y() : [mean: 2.2, median: 2.1, stdev: 3.2, max: 11.1]
Angle difference between orientations : 0.784 radians always.

I was wondering if this is expected, or if I am not comparing in the right frames? Though in the code, the header.frame_id is odom for both the odometry and ground truth messages.

Why is child_frame_id not set for 'odom_msg' (and 'ground_truth_msg') in stageros.cpp?

nav_msgs::Odometry defintion states that twist data should be specified in the coordinate frame given by child_frame_id.

'odom_msg' (and 'ground_truth_msg') variable in stageros.cpp is of type nav_msgs::Odometry and contains twist data. Since, child_frame_id is not set, which co-ordinate frame does the twist data belong to? Also, I would like to know why is the ROS datatype definition not followed here.

Looking forward to your response.

Crashing and speedup simulator whilst running windowless

Hello,
I have been working with stage for a little while now to setup an artificial evolution application in multi-agent systems. However, I'm having issues with the stageros crashing when running the simulator at increased speeds, the evolution can be running fine for 10 minutes or so but then crash and I have to restart.

Errors:
Segmentation fault (core dumped)
or
Error in `/opt/ros/kinetic/lib/stage_ros/stageros': free(): invalid size: 0x00000000090ecd90
Followed by a long list (see below)

I have spent quite a lot of time trying to figure out the cause but haven't made much progress although I feel as though it might be to do with calling the reset service built into stage which I call every time a new individual in the evolution is tested. I understand this feature was added after the official release and might not be suitable when called many times every few seconds.

As another fix I have tried running stage windowless to improve speed up however, when I run stage windowless it seems to ignore the speedup defined in the world file and run at real time speed. If I run the same world file with the gui, it will be run at increased speed.

Any help would be greatly appreciated!

error.txt

stageros does not publish velocities in the odometry

When converting to Stage v4.1.1 the GetGlobalVelocity function was lost for "models", which prevents us from publishing velocities into ROS using the odometry message.

See:

https://github.com/ros-simulation/stage_ros/blob/master/src/stageros.cpp#L378

exported libary doesn't exist

Where is the library stage_ros supposed to come from (

stage_ros/CMakeLists.txt

Line 34 in 7062bd5

LIBRARIES stage_ros ${Stage_LIBRARIES}

)?

I would guess it needs to be removed from the catkin_package() call.

[Feature Request] Reset and Teleport robots service

Hi,
At the moment there is no possibility to move the robots in stage without GUI interaction or driving the robots around. It would be nice to have a "Teleport robot" service to move the robots anywhere on the map and I already implemented a "reset" service which resets the robots positions to the starting positions. This can be handy for running multiple experiments. (#19)
Cheers,
Daniel

Erratic tf behavior due to use_sim_time parameter changed from within stage_ros

stage_ros/src/stageros.cpp

Line 326 in c3abc19

n_.setParam("/use_sim_time", true);

I just took notice that the stage_ros node sets the /use_sim_time parameter to true which causes tf lookups to fail presumably due to clock time mismatches. The error message I get is as follows:

Exception thrown:Lookup would require extrapolation into the past.  Requested time 14.600000000 but the earliest data is at time 1518479321.574429988, when looking up transform from frame [link_a] to frame [odom]

In addition to that, the robot model display in rviz reports tf failures and you see the ghost white links collapsed at the origin.

This issue is inconsistent as there are times when I launch and everything works fine but when it doesn't I restart once or twice (including the roscore) and the issue goes away. My guess is that during launch, some nodes fetch the /use_sim_time parameter before it is set by stage_ros and assign it a false value. By the time that stage_ros sets the parameter to true several nodes are already configured to operate with the real clock time.

I fixed this behavior by adding a <param name=/use_sim_time value=true/> entry to my launch file and this way there's no longer a race condition to set the parameter. I couldn't find any documentation that states this but I think changing a critical parameter from within a node is not the best course of action.

Data interface not showing in i3-wm and cinnamon

When I load stage_ros with my world file in i3-wm or cinnamon (on linux mint 17.1) it doesn't show the lasers or location of the robot (on-click). There is no way of doing this. It does show when I start the simulator in a XFCE session, that made me a bit more sure that it is a problem between stage and certain window managers. I have an Intel HD 4400 video chip with drivers installed. I hope it is not too hard to fix, maybe somebody has already a solution? Thanks in advance.

Position of published camera image is wrong

It seems not possible to set the x or y coordinates of the camera pose and retrieve the correct image. Setting z works though, strangely enough. The cameras will be visible in the GUI at another spot though the image, published over ROS, still shows the original camera image, always from the center of the robot.

I tested this with one camera and by adding two cameras and changing their pose with e.g.:

turtlebot_camera(pose [0.13 1.055 0.05 0])
turtlebot_camera(pose [0.13 -1.055 0.05 0])

Neither does changing the x or y coordinate do anything to the received image over ROS nor do the Images differ if I use several cameras.

I'd love to try to fix that myself though I don't find the code where that is handled. I'd be happy for any suggestions as I'm trying to get around that problem for a deadline.

stage_ros clips ranger intensity values to a maximum value of 256

The maximum intensity in a sensor_msgs::LaserScan coming from stage_ros is clipped to 256. The behavior comes from line 460 in stage_ros.cpp
msg.intensities[i] = (uint8_t)sensor.intensities[i];
By casting to a uint8_t it clips the maximum return of a ranger in stage to 256

Intensities can require more than 8 bits. Stage stores intensity values as doubles internally. On the output side the sensor_msgs::LaserScan message stores intensities as 32 bit floats. In reality the Hokuyo UTM-30LX returns 18 bit intensity values.

car driving model

Hi there,

What is non-honolomic car driving model used in stage_ros? If I send a msg (cmd_vel), how would stage_ros move the vehicle around in the simulator?

My guess is Ackermann car model (https://en.wikipedia.org/wiki/Ackermann_steering_geometry). Is that correct?

Memory leak in stage(_ros) and resulting segfault

Hi stage_ros and Stage maintainers / developers,

I've noticed that the cleanup logic in stage_ros as well as in the Stage simulator is buggy. First of all, the destructor StageNode::~StageNode() is not called when closing the Stage window or exiting the simulation via Ctrl+Q / File -> Quit. I've tested with the following command:
rosrun stage_ros stageros $(rospack find stage_ros)/world/willow-erratic.world
(By adding --prefix "xterm -e gdb -ex run --args" you can launch Stage in gdb to trace the issue more easily.)

Secondly, the memory allocated for the worldvariable (and even others) is never free'd.

I assume the problem is also related to the cleanup/exit logic of the Stage simulator itself (so I am not sure whether this issue should better be placed in the Stage repo): when closing the Stage window, exit(0) is executed which prevents calling dtors of variables with automatic storage duration, like the StageNode sn object created in main().

However, if the destructors would be invoked correctly (by modifying and re-compiling Stage, see modified version here), the simulator crashes with a segmentation fault:

debug: model era command(0.00 0.00 0.00) ($PATH_SHORTENED/libstage/model_position.cc Update)
debug: READING POSITION: [ -11.2770 23.2660 3.1416 ]
($PATH_SHORTENED/libstage/model_position.cc Update)
Stage: User closed the simulation window.
[Thread 0x7fffe77fe700 (LWP 8275) exited]
[Thread 0x7fffed2be700 (LWP 8261) exited]
[Thread 0x7fffecabd700 (LWP 8262) exited]
[Thread 0x7fffdf5fe700 (LWP 8296) exited]
debug: destroying world $PATH_SHORTENED/catkin_ws/src/stage_ros/world/willow-erratic.world >($PATH_SHORTENED/libstage/world.cc ~World)

Thread 1 "stageros" received signal SIGSEGV, Segmentation fault.
0x0000000000000001 in ?? ()
(gdb) bt
#0 0x0000000000000001 in ?? ()
#1 0x00007ffff442ab04 in Stg::Model::~Model (this=0xcbf6a0,
__in_chrg=)
at $PATH_SHORTENED/libstage/model.cc:314
#2 0x00007ffff442ad4a in Stg::Model::~Model (this=0xcbf6a0,
__in_chrg=)
at $PATH_SHORTENED/libstage/model.cc:316
#3 0x00007ffff44a07ce in Stg::Ancestor::~Ancestor (this=0x6e72a0,
__in_chrg=)
at $PATH_SHORTENED/libstage/ancestor.cc:12
#4 0x00007ffff446c41d in Stg::World::~World (this=0x6e72a0,
__in_chrg=)
at $PATH_SHORTENED/libstage/world.cc:166
#5 0x00007ffff449d873 in Stg::WorldGui::~WorldGui (this=0x6e72a0,
__in_chrg=)
at $PATH_SHORTENED/libstage/worldgui.cc:244
#6 0x00007ffff449d930 in Stg::WorldGui::~WorldGui (this=0x6e72a0,
__in_chrg=)
at $PATH_SHORTENED/libstage/worldgui.cc:252
#7 0x0000000000456af9 in StageNode::~StageNode (this=0x7fffffffd2a8)
at $PATH_SHORTENED//catkin_ws/src/stage_ros/src/stageros.cpp:404
#8 0x000000000045b64a in main (argc=2, argv=0x7fffffffd668)
at $PATH_SHORTENED//catkin_ws/src/stage_ros/src/stageros.cpp:786
(gdb)

Please note that there are also some minor changes required in stage_ros to reproduce this, see stage_segfault_patch.txt patch file. In a nutshell:

• replace exit(0) with return 0 to call ctors
• call ros::shutdown() after Stg::World::Run() to ensure thread termination

I highly recommend these changes. I can provide a pull-request (w/o the "delete world" line), if desired.

@rtv Do you have an idea of how to fix this? As already mentioned, the problem seems to be located somewhere deep down in the object hierarchy of the libstage library code (see stack trace above).

stage_ros needs -ldl to be built but it's a order problem

Hi,

building stage_ros in a Debian Wheezy/Jessie/Unstable we have found that it fails because the -ldl of a link to libdl is missed. After dig a bit on the net and test it, I found that it's a problem with the order of the link procedure.

So, in stage_ros/CMakeLists.txt instead of add set(${PROJECT_NAME}_extra_libs dl) just change the order of:

target_link_libraries(stageros
  ${catkin_LIBRARIES}
  ${Boost_LIBRARIES}
  ${FLTK_LIBRARIES}
  ${Stage_LIBRARIES}
  ${${PROJECT_NAME}_extra_libs}
)

by:

target_link_libraries(stageros 
  ${catkin_LIBRARIES} 
  ${Boost_LIBRARIES}
  ${Stage_LIBRARIES} 
  ${FLTK_LIBRARIES}
)

and it compiles without problem.

unpause/pause problem in stage simulator

Hi,everyone, I'm trying to pause or unpause the stage simulator during its running , just like the gazebo service(/gazebo/unpause_physics ,/gazebo/pause_physics),but I can't find such an ROS API ...
I wanna ask Is there a way to achieve such a function? :-)

Screenshots of stage not saved with roslaunch

It seems that stage_ros (indigo) does not save the screenshots in the current folder (although the output says so) when stage_ros is launched through a launch file (while with rosrun works).

Lunar release

Hi stage_ros maintainers!

As you may know the next ROS release Lunar Loggerhead is around the corner 🎉

All dependencies of stage_ros are now released into Lunar! Please consider making a Lunar release soon, stage_ros is part of desktop_full, it is a requirement for the release of ROS Lunar.

Thanks!

stage_ros malloc(): memory corruption

Hi,

I'm trying to run stage_ros with the example worlds but I'm getting malloc(): memory corruption error.
I'm running on ubuntu 14.04 with ros indigo.

When I do: rosrun stage_ros stageros -g willow-erratic.world It works fine.

But when I do: rosrun stage_ros stageros willow-erratic.world It gives me an error:

malloc(): memory corruption

Any ideas of the problem?

Thank you very much

TF name will start '/' when using multi robot configuration due to invalid specification of mapName() function for frame_id

I am using stage_ros with move_base_multi_robot.launch in navigation_tutorials. The launch file does not work properly since tf configuration is invalid.

One of the invalid tf configuration is that frame_id fields of messsages published from stage_ros start with '/' when using multi-robot configuration. ( this is already reported in #68 )

This issue seems comes from mapName() function according to

stage_ros/src/stageros.cpp

Lines 478 to 481 in c3abc19

	if (robotmodel->lasermodels.size() > 1)
	msg.header.frame_id = mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
	else
	msg.header.frame_id = mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));

.
And seeing

stage_ros/src/stageros.cpp

Lines 172 to 227 in c3abc19

	// since stageros is single-threaded, this is OK. revisit if that changes!
	const char *
	StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
	{
	//ROS_INFO("Robot %lu: Device %s", robotID, name);
	bool umn = this->use_model_names;
	if ((positionmodels.size() > 1 ) || umn)
	{
	static char buf[100];
	std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
	if ((found==std::string::npos) && umn)
	{
	snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
	}
	else
	{
	snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
	}
	return buf;
	}
	else
	return name;
	}
	const char *
	StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
	{
	//ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID);
	bool umn = this->use_model_names;
	if ((positionmodels.size() > 1 ) || umn)
	{
	static char buf[100];
	std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
	if ((found==std::string::npos) && umn)
	{
	snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
	}
	else
	{
	snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
	}
	return buf;
	}
	else
	{
	static char buf[100];
	snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
	return buf;
	}
	}

, when the size of positionmodels is lager than 1, frame_id is resolved as like /robot_0/base_laser_link.

This specification is valid for topicnames. But for frame_id, this is invalid.

Un-needed run_depend on fluid

I don't think stage_ros needs the run-depend on fluid. fluid is a UI design package like qt designer and qt uic, and none of code in stage or stage_ros uses it. It should be safe to remove.

Differentiate between world and other robots

Hello,
The ranger models in stage_ros currently return the distance to nearby obstacles. However, they do not indicate if the obstacle being detected is part of the world (i.e. environment), or is another robot.

Ideally, I would like to be able to know whether the ranger is detecting another robot or the environment. Is there a way to do this?
I was considering using color information. For example, making all the robots yellow, and obstacles black, and combining sensor and color data to differentiate between robots and the environment. However, there is no color detection mechanism right now.

Any suggestions are much appreciated!

Define frame_ids as parameters

A great (and simple) addition to stage_ros would be the ability to define all frame_ids as parameters, so that we could use other frame names besides "camera", "odom", "base_footprint", "base_link" and "base_laser_link".

This would allow better portability between code/launch files developed for simulation and for real robots.

Thanks!

Plugins for additional sensor models?

I would suggest adding a plugin support for additional stage sensor model controllers.
I think this can simply be implemented by defining a plugin API with int SubscribeModels() and void WorldCallback() (and any other required functions) and calling those API methods from respective functions in StageNode class.
This way we should be able to add support for additional sensors like blobfinder and fudicial sensor etc from ROS side as well.
If we can agree upon the proposed changes I would be more than happy to contribute in the development.

compiling on saucy is missing dlopen link flags

I'm not sure what changed, but my hydro compile on saucy required adding dl to the target_link_libraries list of libraries. This should be done in a cleaner method calling find_package for dlopen.

Kinetic Release

@gerkey Let me know if you'd like me to run a release for stage_ros in Kinetic.

How to speed up the passage of time of the simulator.

Hello, I want to use stage_ ROS performs reinforcement learning training, but the simulator is real-time, so it takes a long time for me to strengthen learning to train the robot. I want to know how to speed up the passage of time of the simulator.

stage_ros breaks REP 105

Hello
In http://www.ros.org/reps/rep-0105.html you can find: "The base_link can be attached to the base in any arbitrary position or orientation".
In https://github.com/ros-simulation/stage_ros/blob/master/src/stageros.cpp#L490 stage_ros forces the base_link to be on the ground. Because of this I suppose my problem turtlebot/turtlebot_simulator#53 is the fault of the stage_ros.

Regards,
Michael

when using stage simulation some problem occured with global localizaton

Hi,
sorry to trouble you! I'm using the stage simulator doing some path tacking work. I need a global and perfect position to make it, so the topic “base_pose_ground_truth” would be perfect for it. Also In order to eliminating the error of odom, I used the officially recommended "gps" rather than "odom". Still, you will see a large shift in the “odom ” coordinate relative to the global coordinate （map）, especially when the robot is turning.
As far as I know, both of these ("gps" " base_pose_ground_truth" )come from the stage simulator , and the coordinate system mentioned above should not have relative movement(or drift ).
Can you told me what can I do to get the global positioning more precise?

this is the param in stage
——————————
`define difflike_robot position
(
pose [ 0.0 0.0 0.0 0.0 ]

#odom_error [0.00 0.00 999999 999999 999999 0.00]
localization "gps"
localization_origin [ 0 0 0.0 0.0]`

Laser Scan frame-id has "/"

Hello,
I'm using stage_ros with a custom world and two robots in it, and I'm running teb_local_planner independently with it. But I'm getting an error with TEB:
Warning: Invalid argument "/robot_0/base_laser_link" passed to canTransform argument source_frame in tf2 frame_ids cannot start with a '/' like: at line 134 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp [ERROR] [1605531757.746241659, 14.400000000]: TF Exception that should never happen for sensor frame: , cloud frame: /robot_0/base_laser_link, Invalid argument "/robot_0/base_laser_link" passed to lookupTransform argument source_frame in tf2 frame_ids cannot start with a '/' like:
On doing rostopic echo -n 1 /robot_0/base_scan/header, this was the output:
seq: 313 stamp: secs: 31 nsecs: 400000000 frame_id: "/robot_0/base_laser_link"
The "/" in the frame_id seems to be giving the error. I was wondering if there was any method to remove the "/".
Thanks

A plan to support ROS2

Hello. Is there a plan to support ros2?
I couldn't find the ros2 package for this repo.

I found the repository to use Stage on ros2, but it is a wrapper package which need ros1 installation for Stage libraries.
It is not good compared to independent ros2 package.

Thanks.

Programming Color Indication for Robot in Stage

Hi everyone,
I have simulated a roomba in stage simulator. I want to give some kind of color indication while running the robot - green when it is moving, red when it has stopped. One example of doing this can be changing the color of robot itself, other may be putting LEDs on it. Is there a way to implement this programmatically in Stage?

stage_ros is incompatible with the "wander" stage controller

Stage has a "wander" controller for position models that moves them around with crude obstacle avoidance. These do not move when using stage_ros because it sets the speed of all ModelPosition instances to zero in the WorldCallback() line 424.

I want to use the wander controller on some "position" models in a stage world for dynamic obstacles. For the time being I've hacked some code to only call SetPosition on the first robot.

It would be very cool if I could configure stage_ros to only control certain robots. Stage models can have names so maybe a list of robot names?

How to publish sensor msgs and clock messages at a rate higher than 10Hz?

Hello

I am using stage_ros along with navigation2d for p3at. [http://wiki.ros.org/nav2d/Tutorials ] It works fine with the default world parameters of running stage at 10Hz.
I want to publish sensor and clock messages faster than the default of 10Hz (i.e. interval_sim = interval_real = 100ms), I see that the data is published once every worldCallback, which is called with period interval_sim.

When I try to reduce interval_Real or interval_sim, the stage_ros+nav2d simulation stops working - the nav2d module is sometimes not able to find the robot on the map.

I tried adding speedup=2 in the world file, causing the smulation to run at twice the rate, but the sensor&clock is still published at 10Hz (since interval_sim = 100ms).

It'd be great if you could help figure out how to publish sensor,clock data faster, with or without running simulation faster than real time.