Cant Spawn Objects correctly in NRP


#1

Hello,
I am trying to spawn objects in Gazebo via rospy but when running the script all other rospy functionality seizes to work and the skript itselfs also hangs! The object spawns though and can be seen in the simulation.
Cann you tell me what I am doing wrong? This is my code:

import rospy
from gazebo_msgs.srv import SpawnEntity,SpawnEntityRequest
from geometry_msgs.msg import Pose

rospy.wait_for_service('/gazebo/spawn_sdf_entity')
spawn_model_srv = rospy.ServiceProxy("/gazebo/spawn_sdf_entity", SpawnEntity,persistent=False)
initial_pose = Pose()
initial_pose.position.x = 1.7
initial_pose.position.y = 1.0
initial_pose.position.z = 2.7
model = open("sphere.sdf","r")
sdf = model.read()
model_name = "goal"
try:
    spawn_model_srv(model_name, sdf, "", initial_pose, "world")
except rospy.ServiceException as exc:
    print("Service did not process request: " + str(exc))
model.close()

#2

Dear Ecitaku,

Are you running this code from an external node? Actually, the NRP has a framwork for you to spawn objects: the SMACH state machine editor. There is a working example which you might want to check.

Go to the online platform from http://neurorobotics.net or update your local install, then clone a template experiment called “Mini-project 2: Navigate the robot between any two points of its environment” (you can filter with the “mooc” keyword to find it. Clone this experiment and run it. You will see that it spawns red objects automatically. The code for it is in the State machine editor view (4th green button from the left vertical toolbar).

Best regards,
Axel


#3

Dear Axel,
Thanks for the quick answer!
I just looked at the SMACH and the code to spawn is essentially the same as I am using.
I am running my script in my terminal, are you saying that it would work if I added the code to the SMACH instead?
I will try it out now and tell you if it worked!


#4

Hello,
So I just copied the code and added it to the SMACH, but I have the same problem!

import hbp_nrp_excontrol.nrp_states as states
from gazebo_msgs.srv import SpawnEntity, SpawnEntityRequest
from gazebo_msgs.msg import ModelState
from smach import StateMachine
import smach_ros
import smach
from smach import CBState
from hbp_nrp_excontrol.nrp_states import WaitToClockState
import rospy
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Pose, Point, Quaternion
from hbp_nrp_excontrol.logs import clientLogger
from geometry_msgs.msg import Vector3


sphere_xml = """
<?xml version='1.0'?>
<sdf version="1.4">
<model name="my_model">
  <pose>0 0 0.0 0 0 0</pose>
  <static>true</static>
		<link name="ball">
			<pose>-0.983407522236849 -0.2185350300880825 0.14569002630612185 1.570796296000509 -0.0 0.0</pose>
			<inertial>
				<mass>1.0</mass>
				<pose>0 0 0 0 0 0</pose>
				<inertia>
					<ixx>1.0</ixx>
					<ixy>0.0</ixy>
					<ixz>0.0</ixz>
					<iyy>1.0</iyy>
					<iyz>0.0</iyz>
					<izz>1.0</izz>
				</inertia>
			</inertial>
			<visual name="VIS_Sphere">
				<pose>0 0 0 0 -0.0 0.0</pose>
				<geometry>
					<mesh>
						<uri>model://fetus_40w/meshes/visual/VIS_Sphere_2.dae</uri>
						<scale>0.021184567732888127 0.021184567732888127 0.021184567732888127</scale>
					</mesh>
				</geometry>
			</visual>
		</link>
  </model>
</sdf>
"""


@smach.cb_interface(input_keys=[], output_keys=[], outcomes=['finished'])
def draw_cb(userdata):
    global lattice_point_name
    global cylinder_sdf_xml
    global lattice, waypoints
    
    clientLogger.info("Drawing Sphere")
    rospy.wait_for_service('/gazebo/spawn_sdf_entity')
    spawn_model_srv = rospy.ServiceProxy("/gazebo/spawn_sdf_entity", SpawnEntity,persistent=False)
    lattice_msg = SpawnEntityRequest()
    lattice_msg.entity_name = "goal"
    lattice_msg.entity_xml = sphere_xml
    lattice_msg.initial_pose.position.x = 1.7
    lattice_msg.initial_pose.position.y = 1.0
    lattice_msg.initial_pose.position.z = 2.7
    lattice_msg.reference_frame = "world"

    spawn_model_srv(lattice_msg)
    clientLogger.info("Sphere Drawn")
        
    return 'finished'


    
FINISHED = 'FINISHED'
ERROR = 'ERROR'
PREEMPTED = 'PREEMPTED'
sm = smach.StateMachine(outcomes=['DONE'])

with sm:
    smach.StateMachine.add('DRAW', CBState(draw_cb), transitions={'finished': 'DONE'})

In the log console I get this:
[15:47:22] Drawing Sphere
So this means the script hangs after calling the service proxy.
After spawning the sphere nothing gets published on rostopic anymore.


#5

Does the “mini-project” experiment also hangs for you after adding the first red object?


#6

Hey, So I wanted to check and realized I dont have the newest version of NRP installed and tried to update it.
But I keep getting this error:

CDPATH="${ZSH_VERSION+.}:" && cd . && /bin/bash /home/edin/Documents/NRP/mvapich2/confdb/missing aclocal-1.15 -I confdb
/bin/bash: /home/edin/Documents/NRP/mvapich2/confdb/missing: No such file or directory
Makefile:11764: recipe for target 'aclocal.m4' failed
make: *** [aclocal.m4] Error 127
ERROR: Could not uninstall mvapich2 properly, please uninstall manually and rerun this script

Do you maybe know how to fix this?
After that I can tell you if the mini project works locally on my set up.


#7

So I was able to get hbp to run and the mini project is able to span objects without a problem. The problem seems to be with my program then.
But I dont see anything that I did different in my SMACH. Can you see anything?


#8

Hi, Ecitaku,

Based on my experience, they use a different spawn topic ('/spawn_urdf_entity') other than the original '/spawn_urdf_model'. For sdf, maybe it is the same case. Please try to check which is the correct topic name using rostopic list.

For my case, I solved this problem by implementing a spawn_robot_model.py in replace of the previous spawn_model script by myself, which points to the correct topic name.

Hope this message could help.

Best regards,

Rui