No reaction when controlling a second model with a second node via ros in my gazebo world

asked 2019-06-23 12:01:47 -0500

Marcelpanz gravatar image

updated 2019-06-24 01:13:30 -0500

Hey,

I am still new to gazebo and tried to move a model in my gazebo world. I succesfully did this for a first model and wanted to integrate a second one. Here, the second object isn´t moving at all, although I am using the exactly same node (only with changed names and coordinates). When I have a look at the rqt graph, I can see that the first node doesnt get any information from the /gazebo/model_states node but the second one does (altough the first one is the one that runs well?): image description

Here is the part with the obstacles from the .world file:

      <model name="obstacle_1">
      <pose>2.7 2.5 0.25 1.57 0 1.57</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
          <cylinder>
            <radius>0.25</radius>
            <length>0.5</length>
          </cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
          <cylinder>
            <radius>0.25</radius>
            <length>0.3</length>
          </cylinder>
          </geometry>
        </visual>
      </link>
    </model>

    <model name="obstacle_2">
      <pose>2.6 -2.5 0.25 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
          <box>
            <size>0.2 0.2 0.2</size>
          </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
          <box>
            <size>0.2 0.2 0.2</size>
          </box>
          </geometry>
        </visual>
      </link>
    </model>

Here is the code for the two nodes:

 #!/usr/bin/env python

import rospy
import time
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] == 'obstacle_1':
                    obstacle_1 = ModelState()
                    obstacle_1.model_name = model.name[i]
                    obstacle_1.pose = model.pose[i]
                    obstacle_1.twist = model.twist[i]

                    if obstacle_1.pose.position.y > 2.5:
                        obstacle_1.twist.linear.y = -0.25

                    elif obstacle_1.pose.position.y < 2.5 and obstacle_1.pose.position.y > -1.5 and obstacle_1.twist.linear.y <0:
                        obstacle_1.twist.linear.y = -0.25

                    elif obstacle_1.pose.position.y < -1.5:
                        obstacle_1.twist.linear.y = 0.25

                    elif obstacle_1.pose.position.y < 2.5 and obstacle_1.pose.position.y > -1.5 and obstacle_1.twist.linear.y >0:
                        obstacle_1.twist.linear.y = 0.25


                    self.pub_model.publish(obstacle_1)
                    time.sleep(0.1)

def main():
    rospy.init_node('simulation_5_obstacle_1')
    try:
        combination = Combination()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()

Here is the code for the second node:

#!/usr/bin/env python

import rospy
import time
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] == 'obstacle_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 ...
(more)
edit retag flag offensive close merge delete