Hi, I'm trying to implement a simple planer using wavefront. I'm using gazebo0.8 and player 2.1.1. Is there a posibilty to run wavefront not with the position provided by the amcl driver, but with the real position auf the simulation? I just want to know wether wavefront plans a correct path. If i start my script the robot moves to the right position provided by the amcl but this position is doesnt correspond to the position the robot has in the simulation. This is my code. What I'm doing wrong?
driver ( name "gazebo" provides ["simulation:0"] plugin "libgazeboplugin" alwayson 1 ) driver ( name "gazebo" provides ["odometry:::position2d:0"] gz_id "robot1" ) driver ( name "gazebo" provides ["laser:0"] gz_id "laser_iface_0" ) driver ( name "vfh" provides ["position2d:1"] requires ["position2d:0" "laser:0"] #safety_dist 0.1 distance_epsilon 0.3 angle_epsilon 5 ) driver ( name "mapfile" provides ["map:0"] filename "wavefront.pgm" resolution 0.1 ) driver ( name "amcl" provides ["position2d:2"] init_pose [0.0 0.0 0.0] requires ["odometry:::position2d:1" "laser:0" "laser:::map:0"] ) driver ( name "wavefront" provides ["planner:0"] requires ["output:::position2d:1" "input:::position2d:2" "map:0"] safety_dist 0.15 distance_epsilon 0.5 angle_epsilon 10 ) <?xml version="1.0"?> <gazebo:world xmlns:xi="http://www.w3.org/2001/XInclude"; xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"; xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"; xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"; xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"; xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"; xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"; xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"; xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"; xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"; xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"; xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"; xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"; xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"; > <verbosity>5</verbosity> <physics:ode> <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> <erp>0.3</erp> </physics:ode> <rendering:gui> <type>fltk</type> <size>800 600</size> <pos>0 0</pos> <xyz>0 0 0</xyz> <rpy>0 0 0</rpy> </rendering:gui> <rendering:ogre> <ambient>0.5 0.5 0.5 1.0</ambient> <sky> <material>Gazebo/CloudySky</material> </sky> <fog> <color>1.0 1.0 1.0</color> <linearStart>10</linearStart> <linearEnd>100</linearEnd> </fog> </rendering:ogre> <!-- Ground Plane --> <model:physical name="plane1_model"> <xyz>0 0 0</xyz> <rpy>0 0 0</rpy> <static>true</static> <body:plane name="plane1_body"> <geom:plane name="plane1_geom"> <normal>0 0 1</normal> <size>2000 2000</size> <segments>10 10</segments> <uvTile>100 100</uvTile> <material>Gazebo/Grey</material> </geom:plane> </body:plane> </model:physical> <model:physical name="wall0_model"> <body:box name="wall_0_body"> <xyz> 0.00 10.00 0.50</xyz> <rpy> 0.00 0.00 0.00</rpy> <static>true</static> <geom:box name="wall_0_geom"> <mesh>default</mesh> <size> 20.00 0.20 1.00</size> <visual> <size> 20.00 0.20 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <model:physical name="wall1_model"> <body:box name="wall_1_body"> <xyz> 0.00 -10.00 0.50</xyz> <rpy> 0.00 0.00 0.00</rpy> <static>true</static> <geom:box name="wall_1_geom"> <mesh>default</mesh> <size> 20.00 0.20 1.00</size> <visual> <size> 20.00 0.20 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <model:physical name="wall2_model"> <body:box name="wall_2_body"> <xyz> -10.10 0.00 0.50</xyz> <rpy> 0.00 0.00 90.00</rpy> <static>true</static> <geom:box name="wall_2_geom"> <mesh>default</mesh> <size> 20.00 0.20 1.00</size> <visual> <size> 20.00 0.20 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <model:physical name="wall3_model"> <body:box name="wall_3_body"> <xyz> 10.10 0.00 0.50</xyz> <rpy> 0.00 0.00 90.00</rpy> <static>true</static> <geom:box name="wall_3_geom"> <mesh>default</mesh> <size> 20.00 0.20 1.00</size> <visual> <size> 20.00 0.20 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <model:physical name="wall4_model"> <body:box name="wall_4_body"> <xyz> 4.25 6.00 0.50</xyz> <rpy> 0.00 0.00 90.0</rpy> <static>true</static> <geom:box name="wall_4_geom"> <mesh>default</mesh> <size> 4.00 0.50 1.00</size> <visual> <size> 4.00 0.50 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <model:physical name="wall5_model"> <body:box name="wall_5_body"> <xyz> -3.25 -2.00 0.50</xyz> <rpy> 0.00 0.00 90.0</rpy> <static>true</static> <geom:box name="wall_5_geom"> <mesh>default</mesh> <size> 8.00 0.50 1.00</size> <visual> <size> 8.00 0.50 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <model:physical name="wall6_model"> <body:box name="wall_5_body"> <xyz> 5.00 -0.25 0.50</xyz> <rpy> 0.00 0.00 0.00</rpy> <static>true</static> <geom:box name="wall_5_geom"> <mesh>default</mesh> <size> 6.00 0.50 1.00</size> <visual> <size> 6.00 0.50 1.00</size> <material>Gazebo/PioneerBody</material> <mesh>unit_box</mesh> </visual> </geom:box> </body:box> </model:physical> <!-- Include the complete model described in the .model file This assumes the root node is a <model:...> --> <!-- <include embedded="false"> <xi:include href="pioneer2dx.model" /> </include> --> <model:physical name="pioneer2dx_model1"> <xyz>0 0 0.14</xyz> <rpy>0.0 0.0 0.0</rpy> <python>pioneer2dx</python> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> <rightJoint>right_wheel_hinge</rightJoint> <wheelSeparation>0.39</wheelSeparation> <wheelDiameter>0.15</wheelDiameter> <torque>5</torque> <interface:position name="robot1"/> </controller:differential_position2d> <model:physical name="laser"> <xyz>0.15 0 0.18</xyz> <attach> <parentBody>chassis_body</parentBody> <myBody>laser_body</myBody> </attach> <include embedded="true"> <xi:include href="models/sicklms200.model" /> </include> </model:physical> <include embedded="true"> <xi:include href="models/pioneer2dx.model" /> </include> </model:physical> <!-- White Directional light --> <model:renderable name="directional_white"> <light> <type>directional</type> <direction>0 -0.6 -0.4</direction> <diffuseColor>1.0 1.0 1.0</diffuseColor> <specularColor>0.2 0.2 0.2</specularColor> <attenuation>1000 1.0 0.0 0</attenuation> </light> </model:renderable> </gazebo:world> #!/usr/bin/env python import math, time,sys from playerc import * # Create a client object client = playerc_client(None, 'localhost', 6665) # Connect it if client.connect() != 0: raise playerc_error_str() # Create a proxy for position2d:0 position0 = playerc_position2d(client,0) if position0.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() # Retrieve the geometry if position0.get_geom() != 0: raise playerc_error_str() #print 'Robot size: (%.3f,%.3f)' % (position0.size[0], position0.size[1]) print 'Robot position: ', position0.px, position0.py, position0.pa map0 = playerc_map(client,0) if map0.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() #get actual map map0.get_map() # Create a proxy for laser:0 laser0 = playerc_laser(client,0) if laser0.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() # Retrieve the geometry if laser0.get_geom() != 0: raise playerc_error_str() #print 'Laser pose: (%.3f,%.3f,%.3f)' % (laser0.pose[0],laser0.pose[1],laser0.pose[2]) # Create a proxy for position2d:1 position1 = playerc_position2d(client,1) if position1.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() # Create a proxy for position2d:2 position2 = playerc_position2d(client,2) if position2.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() planner0 = playerc_planner(client,0) if planner0.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() simulation0 = playerc_simulation(client,0) if simulation0.subscribe(PLAYERC_OPEN_MODE) != 0: raise playerc_error_str() position0.enable(1) planner0.enable(1); #Zielposition setzen planner0.set_cmd_pose(7,-3,0) try: while True: client.read() print 'reale Robotor Position: (%.3f,%.3f,%.3f)' % (position0.px,position0.py,position0.pa) print 'Partikelfilter Position: (%.3f,%.3f,%.3f)' % (position2.px,position2.py,position2.pa) except KeyboardInterrupt: # Clean up position0.set_cmd_vel(0.0, 0.0, 0.0, 1) position0.enable(0) position0.unsubscribe() position1.unsubscribe() position2.unsubscribe() map0.unsubscribe() planner0.enable(0) planner0.unsubscribe() laser0.unsubscribe() client.disconnect() __________________________________________________ Do You Yahoo!? Sie sind Spam leid? Yahoo! Mail verfügt über einen herausragenden Schutz gegen Massenmails. http://mail.yahoo.com ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-gazebo mailing list [email protected] https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
