Unable to control more than one object in gazebo with one node
Hey,
I just tried to build up a changing world to train my Turtlebot3 in it. The idea is, that everytime when the robot spawns to (0, 0) again, parts of the layout and obstacles shall change. Here I try to switch positions and movement directions of my three obstacles when the Robot respawns. The problem is, only the first obstacle is moving. The other one don´t move at all. My question is, can I communicate with all obstacles from one node? In the following you can see my node code and a part of the .world file. Node:
#!/usr/bin/env python
import rospy
import time
import random
import math
from gazebo_msgs.msg import ModelState, ModelStates
class Combination():
def __init__(self):
self.pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1)
self.moving()
def moving(self):
state = 0
while not rospy.is_shutdown():
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
for i in range(len(model.name)):
if model.name[i] == 'turtlebot3_burger':
turtlebot_pos= ModelState()
turtlebot_pos.model_name=model.name[i]
turtlebot_pos.pose = model.pose[i]
if abs(turtlebot_pos.pose.position.y)<0.01 and abs(turtlebot_pos.pose.position.x)<0.02:
for i in range(len(model.name)):
if model.name[i] == 'obstacle_1':
a = random.randint(1,2)
obstacle_1 = ModelState()
obstacle_1.model_name = model.name[i]
obstacle_1.pose = model.pose[i]
obstacle_1.twist = model.twist[i]
if a==1:
obstacle_1.pose.position.x=2.5
obstacle_1.pose.position.y=2.5
if obstacle_1.pose.position.y > 2.2:
obstacle_1.twist.linear.y = -0.25
elif obstacle_1.pose.position.y < 2.2 and obstacle_1.pose.position.y > -1.2 and obstacle_1.twist.linear.y <0:
obstacle_1.twist.linear.y = -0.25
elif obstacle_1.pose.position.y < -1.2:
obstacle_1.twist.linear.y = 0.25
elif obstacle_1.pose.position.y < 2.2 and obstacle_1.pose.position.y > -1.2 and obstacle_1.twist.linear.y >0:
obstacle_1.twist.linear.y = 0.25
if a==2:
obstacle_1.pose.position.x=2.5
obstacle_1.pose.position.y=2.5
if obstacle_1.pose.position.x > 2.2:
obstacle_1.twist.linear.x = -0.25
elif obstacle_1.pose.position.x < 2.2 and obstacle_1.pose.position.x > 0 and obstacle_1.twist.linear.x <0:
obstacle_1.twist.linear.x = -0.25
elif obstacle_1.pose.position.x < 0:
obstacle_1.twist.linear.x = 0.25
elif obstacle_1.pose.position.x < 2.2 and obstacle_1.pose.position.x > 0 and obstacle_1.twist.linear.x >0:
obstacle_1.twist.linear.x = 0.25
if model.name[i] == 'obstacle_2':
b = random.randint(1,2)
obstacle_2 = ModelState()
obstacle_2.model_name = model.name[i]
obstacle_2.pose = model.pose[i]
obstacle_2.twist = model.twist[i]
if obstacle_2.pose.position.x > 2.5:
obstacle_2.twist.linear.x = -0.25
elif obstacle_2.pose.position.x < 2.5 and obstacle_2.pose.position.x > -0.5 and obstacle_2.twist.linear.x <0:
obstacle_2.twist.linear.x = -0.25
elif obstacle_2.pse.position.x < -0.5:
obstacle_2.twist.linear.x = 0.25
elif obstacle_2.pose.position.x < 2.5 and obstacle_2.pose.position.x > -0.5 and ...