Gazebo simulation stop working after moving few model

paoloelle · November 23, 2024, 12:04pm

I’m running a simulation in Gazebo Fortress with the TurtleBot4 robot. In this environment a need to stop and restart the simulation multiple times and move the objects that are just a cylindrical model. I need to check the positions of the objects every time I move them, to do it I use the function called generate_random_position_object in the following script. The problem is when I do it with several objects the simulation stops working properly, that is the robot doesn’t move anymore even if it receives the linear and angular velocities commands different from zeros.

I don’t understand why this is possible. Also, everything works properly if I run the same simulation with moving only 5 objects. I don’t know if the problem is related to the number of calls to a service that Gazebo has to handle. From the log file, as far as I can see, the call to the service is executed properly since it returns the following statement data: true.

I’m running the simulation on a HPC cluster, I specify this since the same simulation doesn’t have any problem if I execute it on my local PC.

#!/bin/bash

source /opt/ros/humble/setup.bash
source /workspace/turtlebot4_minimize_surprise/install/setup.bash 


SIMULATION_TIME=60

# set spawn limit turtlebot4
X_MIN_TB=-1.8
X_MAX_TB=1.8
Y_MIN_TB=-0.2
Y_MAX_TB=3.5

# set spawn limit objects
X_MIN_OBJ=-1.8
X_MAX_OBJ=1.8
Y_MIN_OBJ=2.6
Y_MAX_OBJ=3.5

# variables to store turtlebot4 starting position and orientation
TURTLEBOT_X=0
TURTLEBOT_Y=0
TURTLEBOT_Z=0
TURTLEBOT_QX=0
TURTLEBOT_QY=0
TURTLEBOT_QZ=0
TURTLEBOT_QW=1

# variables to store objects position
OBJECT1_X=0
OBJECT1_Y=0
OBJECT2_X=0
OBJECT2_Y=0
OBJECT3_X=0
OBJECT3_Y=0
OBJECT4_X=0
OBJECT4_Y=0
OBJECT5_X=0
OBJECT5_Y=0
OBJECT6_X=0
OBJECT6_Y=0
OBJECT7_X=0
OBJECT7_Y=0

OBJECT_Z=0.2

# array to store object positions and orientations
declare -A OBJECTS

# function to spawn random position turtlebot
generate_random_position_turtlebot() {
    local x_min=$1
    local x_max=$2
    local y_min=$3
    local y_max=$4

    # generate random coordinate within limits
    local x=$(awk -v min=$x_min -v max=$x_max -v seed=$RANDOM 'BEGIN{srand(seed); print min+rand()*(max-min)}')
    local y=$(awk -v min=$y_min -v max=$y_max -v seed=$RANDOM 'BEGIN{srand(seed); print min+rand()*(max-min)}')


    # generate z coordiante based on y values
    local z
    #if (( $(echo "$y > 2.6" | bc -l) )); then # source limit
    #    z=0.11
    #elif (( $(echo "$y >= 1.5 && $y <= 2.6" | bc -l) )); then # slope limits
    #    z=0.09
    #else # cache and nest limit
    #    z=0.04
    #fi

    z=0.15

    # Assegna i valori generati alle variabili globali
    TURTLEBOT_X=$x
    TURTLEBOT_Y=$y
    TURTLEBOT_Z=$z
}

# function to spawn a random orientation (z-axis) turtlebot
generate_random_orientation_turtlebot() {
    # Generate a random yaw (rotation around z-axis) between 0 and 2*pi
    local yaw=$(awk 'BEGIN{srand(); print rand() * 2 * 3.14159265359}')

    # Convert the yaw to a quaternion (assuming no pitch or roll)
    TURTLEBOT_QX=0
    TURTLEBOT_QY=0
    TURTLEBOT_QZ=$(awk -v yaw=$yaw 'BEGIN{print sin(yaw / 2)}')
    TURTLEBOT_QW=$(awk -v yaw=$yaw 'BEGIN{print cos(yaw / 2)}')
}

generate_random_position_object() {
    local obj_id=$1

    while true; do
        local x=$(awk -v min=$X_MIN_OBJ -v max=$X_MAX_OBJ -v seed=$RANDOM 'BEGIN{srand(seed); print min+rand()*(max-min)}')
        local y=$(awk -v min=$Y_MIN_OBJ -v max=$Y_MAX_OBJ -v seed=$RANDOM 'BEGIN{srand(seed); print min+rand()*(max-min)}')


        # Check if the generated position is too close to the TurtleBot4
        local distance_to_turtlebot
        distance_to_turtlebot=$(awk -v x1=$TURTLEBOT_X -v y1=$TURTLEBOT_Y -v x2=$x -v y2=$y 'BEGIN{print sqrt((x2-x1)^2 + (y2-y1)^2)}')
        if (( $(echo "$distance_to_turtlebot <= 0.5" | bc -l) )); then
            continue
        fi

        # Check if the generated position is too close to any other objects
        local valid=true
        for existing_obj in OBJECT1 OBJECT2 OBJECT3 OBJECT4 OBJECT5 OBJECT6 OBJECT7; do
            local obj_x=${existing_obj}_X
            local obj_y=${existing_obj}_Y
            local distance_to_obj
            distance_to_obj=$(awk -v x1=${!obj_x} -v y1=${!obj_y} -v x2=$x -v y2=$y 'BEGIN{print sqrt((x2-x1)^2 + (y2-y1)^2)}')
            if (( $(echo "$distance_to_obj <= 0.2" | bc -l) )); then
                valid=false
                break
            fi
        done

        if [ "$valid" = true ]; then
            eval "OBJECT${obj_id}_X=$x"
            eval "OBJECT${obj_id}_Y=$y"
            break
        fi
    done

    ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object$obj_id' position: { x: $(eval echo \$OBJECT${obj_id}_X), y: $(eval echo \$OBJECT${obj_id}_Y), z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000    
    
    echo "respawned object${obj_id}"

}





simulation_id=$1
genome_id=$2

echo -e "\n---------------------------\nGenome ID: $genome_id, Simulation ID: $simulation_id\n---------------------------"

export ROS_DOMAIN_ID=$simulation_id && export IGN_PARTITION=$simulation_id

fastdds shm clean # https://github.com/eProsima/Fast-DDS/issues/2003#issuecomment-1160245640

while true; do
    echo "Update genome_id"
    timeout 10 ros2 param set /ann_controller genome_id  $genome_id # set the genome_id
    result=$?
    echo "Exit code: $result"
    if [ $result -eq 0 ]; then break; fi
    sleep 2
done

echo "set new genome"
sleep 5

while true; do
    timeout 10 ros2 param set /ann_controller update_genome_id True
    result=$?
    echo "Exit code: $result"
    if [ $result -eq 0 ]; then break; fi
    sleep 2
done

echo "enabled genome"

# generate random position turtlebot4
generate_random_position_turtlebot $X_MIN_TB $X_MAX_TB $Y_MIN_TB $Y_MAX_TB
sleep 2

# generate random orientation turtlebot4
generate_random_orientation_turtlebot
sleep 2


# # reset turtlebot4 position
echo "Respawning turtlebot4"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'turtlebot4' position: { x: $TURTLEBOT_X, y: $TURTLEBOT_Y, z: $TURTLEBOT_Z } orientation: { x: $TURTLEBOT_QX, y: $TURTLEBOT_QY, z: $TURTLEBOT_QZ, w: $TURTLEBOT_QW}" --timeout 2000
#sleep 2


# Generate and set positions for objects individually
echo "Respawning object1"
generate_random_position_object "1"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object1' position: { x: $OBJECT1_X, y: $OBJECT1_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

echo "Respawning object2"
generate_random_position_object "2"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object2' position: { x: $OBJECT2_X, y: $OBJECT2_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

echo "Respawning object3"
generate_random_position_object "3"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object3' position: { x: $OBJECT3_X, y: $OBJECT3_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

echo "Respawning object4"
generate_random_position_object "4"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object4' position: { x: $OBJECT4_X, y: $OBJECT4_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

echo "Respawning object5"
generate_random_position_object "5"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object5' position: { x: $OBJECT5_X, y: $OBJECT5_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

echo "Respawning object6"
generate_random_position_object "6"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object6' position: { x: $OBJECT6_X, y: $OBJECT6_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

echo "Respawning object7"
generate_random_position_object "7"
#ign service -s /world/arena/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --req "name: 'object7' position: { x: $OBJECT7_X, y: $OBJECT7_Y, z: $OBJECT_Z} orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }" --timeout 2000
#sleep 2

# start simulation
echo -e "start simulation\n"
ign service -s /world/arena/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 5000 --req 'pause: false'

#sleep $SIMULATION_TIME & # simulation time 


# record the rosbag
ros2 bag record -o log2/$genome_id/bag \
    /model/turtlebot4/pose \
    /cmd_vel \
    /cliff_sensor_front_left \
    /cliff_sensor_front_right \
    /cliff_sensor_side_left \
    /cliff_sensor_side_right \
    /light_sensor_frontL \
    /light_sensor_frontR \
    /light_sensor_back \
    /hazard_detection \
    /scan \
    /model/object1/pose \
    /model/object2/pose \
    /model/object3/pose \
    /model/object4/pose \
    /model/object5/pose \
    /model/object6/pose \
    /model/object7/pose &

BAG_PID=$!
sleep $SIMULATION_TIME

kill $BAG_PID

sleep 2

echo "save score"
ros2 topic echo /fitness_topic --once > log2/$genome_id/score.txt # store the score
sleep 2

echo "stop simulation"
ign service -s /world/arena/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 5000 --req 'pause: true' # stop simulation
sleep 2

Topic		Replies	Views
Gazebo simulation performance problem on long run Help ros2 , ignition , fortress , gazebo	0	18	December 2, 2024
PosePublisher Plugin unexpected behaviour Help ignition , fortress , gazebo	0	188	February 14, 2024
Expert help needed (hiring) Jobs gazebo7	1	605	September 26, 2021
[GSoC] Demos for Ignition knowledge base project discussion Projects helloworld , ros1	3	652	April 27, 2020
Gazebo ros2 controller, cannot moving drone Help ros2 , gazebo	4	172	September 24, 2024

Gazebo simulation stop working after moving few model

Related topics