[SOLVED] Ball image delayed after ball position

#1

Hello,

in my experiment (quite simplified description), there is an iCub robot and a ball which starts at a given position and then the ball looms to the robot until it hits it. Then the ball moves at the starting point and flyes to the robot again.

I have a problem that an image with the ball is SOMETIMES delayed after the position of the ball. For example, in the image, the ball is hitting the robot, but I get x,y,z position of the image which corresponds to the ball’s starting point. It means that x,y,z position is approx. 1-2 steps ahead to the camera image. It does not happen all the time, but approx. in one case of five. Code of sensor collector is bellow.

  1. Is there any general advice what I can try to fix it?
  2. It seems that ball_time (see the code bellow) is system time (e.g. 1611585551.932694) in contrast to camera_time which is simulation time (e.g. 10.80001… 10.8 secs from the start of the simulation). Is it possible to get simulation time of ball position service? I mean to know simulation time in which x,y,z position was obtained and then use it to find corresponding camera image (camera simulation time is known).

Thank you very much!

Zdenek

listener = tf.TransformListener()
rospy.wait_for_service("/gazebo/get_model_state")
service_proxy_get = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState, persistent=True)

@nrp.MapVariable("position_service", initial_value=service_proxy_get)
@nrp.MapCSVRecorder("visual_recorder",
                    filename="visual_data.csv",
                    headers=["Left Camera", "Ball X position", "Ball Y position", "Ball Z position", "Camera time", "Ball time"])
@nrp.MapRobotSubscriber("camera_left",
                        Topic("/icub/icub_model/left_eye_camera/image_raw",
                              sensor_msgs.msg.Image))
...
@nrp.Robot2Neuron(triggers="camera_left")
def sensor_collector(t, visual_recorder, skin_recorder, skin, camera_left, position_service, ball_time, position_x, position_y, position_z, camera_time, skin_time):
    ball_msg = [position_service.value('ball', 'world')]
    camera_left_msg = [camera_left.value]

    ball_time = ball_msg[0].header.stamp.to_sec()

    position_x = ball_msg[0].pose.position.x
    position_y = ball_msg[0].pose.position.y
    position_z = ball_msg[0].pose.position.z

    camera_time = camera_left_msg[0].header.stamp.to_sec()

    visual_recorder.record_entry(camera_left_msg[0], position_x, position_y, position_z, camera_time, ball_time)
#2

Dear Zdenek,

If you spawn the ball from a state machine, you spawn it from ROS and ROS might be using real time on your system. To change that, call

rosparam set use_sim_time true

in the terminal before calling cle-start. (I assume you have a local NRP installation).

This is a fix from a previously reported similar issue:
https://hbpneurorobotics.atlassian.net/browse/NUIT-93
The official fix is not yet deployed:

Best regards
Axel

#3

Dear Axel,
thank you for your answer.
I do not have a local NRP installation. Is there any way how I can fix it in online NRP?

Best regards,

Zdenek

#4

Dear @vonarnim,
if there is no way how I can set rosparam set use_sim_time true in online NRP, is there any way how I can do it in Docker NRP installation?

Thank you in advance.

Best regards,

Zdenek

#5

Dear Zdenek,

Yes, while waiting on the online NRP to be deployed with the fix, it is a good idea to install it locally from Docker. Then, you open a bash in the backend container:

sudo docker exec -i -t nrp /bin/bash

and you can apply the fix mentioned earlier in the $HBP/user-script directory. You should have read access to the pull request. If not, I can send you a zip with the changed files.

Finally, close your container-terminal and from your own machine’s terminal, restart your container

cd $HBP/admin-scripts
./nrp_installer.sh start

tell me if that works for you,
Axel

#6

Dear @vonarnim,

thank you very much. I followed the proposed solution (see the command sequence bellow) and unfortunately, I was not successful. Ball position still used system time. It seems, that use_sim_time=true is lost after the restart of the container (command sudo ./nrp_installer.sh start bellow). I’m docker beginner, so maybe I did not save the setting use_sim_time=true (do not know how… I tried some attempts but I was not successful). Do you think that the problem is that the setting of use_sim_time=true is not permanent and is lost after the container restart or the problem is somewhere else?

Thank you.

Best,

Zdenek

zdenek@zdenek-ThinkPad-T460p:~/nrp$ sudo ./nrp_installer.sh install

zdenek@zdenek-ThinkPad-T460p:~/nrp$ sudo docker exec -i -t nrp /bin/bash
bbpnrsoa@eba235641262:~/nrp/src$ cd user-scripts/
bbpnrsoa@eba235641262:~/nrp/src/user-scripts$ rosparam set use_sim_time true
bbpnrsoa@eba235641262:~/nrp/src/user-scripts$ exit

zdenek@zdenek-ThinkPad-T460p:~/nrp$ sudo ./nrp_installer.sh start
#7

Hm, ok, that might be. Sorry for that.
Then you have to hack the rosparam in the appropriate start command in the container. Starting NRP is done by a service called supervisorctl. Edit the ini file corresponding to the main backend part to add rosparam.

sudo docker exec -i -t nrp /bin/bash   # open a term in the backend container
cd /etc/supervisord.d
sudo nano ros-simulation-factory_app.ini   # edit the so-called CLE start file

Now insert the rosparam command in the “command” line (should be the 2nd line in the file). The 2 first lines should now look like:

[program:ros-simulation-factory_app]
command=/bin/bash -c "source $HBP/user-scripts/nrp_variables && rosparam set use_sim_time true && export ROS_IP=...  # and so on

Save and exit the editor (CTRL+S, CTRL+X) and restart services:

sudo supervisorctl restart all

After a couple of seconds, your services should be started, which you can check by running

sudo supervisorctl status

Finally, open your regular browser and try to run your simulation. For me, it runs as usual, no errors. This change should be permanent as well if you exit the container terminal and restart your container eventually.

Best regards
Axel

#8

Dear Axel,
thank you very much! I did what you proposed (and then reseted NRP by ./nrp_installer.sh start) and it seems that it works correctly :slight_smile:

Best regards,

Zdenek