Ros learns Python read-write text files

Many times we will need to save some files to record relevant information.

So how to save our first project is to want to save Rviz click on the waypoints point.

The code is as follows:

ImportYamlImportRospyImportgeometry_msgs.msg as Geometry_msgsclassWaypointgenerator (object):def __init__(self, filename): Self._sub_pose= Rospy. Subscriber ('Clicked_point', Geometry_msgs. pointstamped, Self._process_pose, queue_size=1) self._waypoints=[] Self._filename=filenamedef_process_pose (Self, msg): P=Msg.point Data={} data['frame_id'] =msg.header.frame_id data['pose'] ={} data['pose']['position'] = {'x': p.x,'y': P.y,'Z': 0.0} data['pose']['Orientation'] = {'x': 0,'y': 0,'Z': 0,'W': 1} data['name'] ='%s_%s'%(p.x, P.Y) self._waypoints.append (data) Rospy.loginfo ("Clicked: (%s,%s,%s)"%(p.x, P.Y, p.z))def_write_file (self): ways={} ways['waypoints'] =self._waypoints with open (Self._filename,'W') as F:f.write (Yaml.dump (Ways, Default_flow_style=False)) defSpin (self): Rospy.spin () self._write_file ()if __name__=='__main__': Rospy.init_node ('Waypoint_generator') filename= Rospy.get_param ('~filename') G=waypointgenerator (filename) rospy.loginfo ('Initialized') G.spin () Rospy.loginfo ('Byebye')

This node is subscribed to the publish point inside the Rviz for storage.

The make file should look like this:

Cmake_minimum_required (VERSION 2.8.3) project (waypoint_generator) find_package (Catkin required  components Geometry_msgs  Rospy) catkin_python_setup () catkin_package () Install (PROGRAMS  scripts/  generator.py  DESTINATION ${catkin_package_bin_destination})

We are writing a launch file to write the file storage location.

<launch><node pkg="Waypoint_generator"Type="generator.py"Name="Waypoint_generator"output=" Screen"> <param name="filename"Value="$ (Find waypoint_generator)/output.txt"/></node><node pkg="Show_path"Type="Showpath"Name="Showpath"/></launch>

The storage location is in the main directory under the package.

Here is the read TXT file and published as goal, which also includes the use of actionlib.

ImportRandomImportRospyImportActionlibImportWaypoint_touring.utils as UtilsImportmove_base_msgs.msg as Move_base_msgsImportvisualization_msgs.msg as Viz_msgsclassTourmachine (object):def __init__(self, filename, random_visits=false, repeat=False): self._waypoints=utils.get_waypoints (filename) action_name='Move_base'Self._ac_move_base=Actionlib. Simpleactionclient (Action_name, Move_base_msgs. movebaseaction) Rospy.loginfo ('Wait for%s server'%action_name) Self._ac_move_base.wait_for_server self._counter=0 Self._repeat=Repeat Self._random_visits=random_visitsifself._random_visits:random.shuffle (self._waypoints) Self._pub_viz_marker= Rospy. Publisher ('viz_waypoints', Viz_msgs. Markerarray, queue_size=1, latch=True) Self._viz_markers=utils.create_viz_markers (self._waypoints)defMove_to_next (self): P=self._get_next_destination ()if  notP:rospy.loginfo ("Finishing Tour")            returnTrue Goal=utils.create_move_base_goal (P) rospy.loginfo ("Move to%s"% p['name']) Self._ac_move_base.send_goal (goal) Self._ac_move_base.wait_for_result () result=Self._ac_move_base.get_result () rospy.loginfo ("Result:%s"%result)returnFalsedef_get_next_destination (self):ifSelf._counter = =Len (self._waypoints):ifSelf._repeat:self._counter=0ifself._random_visits:random.shuffle (self._waypoints)Else: Next_destination=None next_destination=Self._waypoints[self._counter] Self._counter= Self._counter + 1returnnext_destinationdefSpin (self): Rospy.sleep (1.0) Self._pub_viz_marker.publish (self._viz_markers) finished=False while  notRospy.is_shutdown () and  notfinished:finished=Self.move_to_next () rospy.sleep (2.0)if __name__=='__main__': Rospy.init_node (' Tour') filename= Rospy.get_param ('~filename') Random_visits= Rospy.get_param ('~random', False) Repeat= Rospy.get_param ('~repeat', False) m=tourmachine (filename, random_visits, repeat) rospy.loginfo ('Initialized') M.spin () Rospy.loginfo ('Bye Bye')

and write a launch file to read the TXT location,

<launch> <arg name="Repeat"default="true"/> <arg name="Random"default="false"/> <arg name="filename"default="$ (Find waypoint_generator)/output.txt"/> <node pkg="Rviz"Type="Rviz"Name="Rviz"/> <node pkg="waypoint_touring"Name="waypoint_touring"Type="tour.py"output=" Screen"> <param name="filename"Value="$ (arg filename)"/> <param name="Repeat"Value="$ (arg repeat)"/> <param name="Random"Value="$ (arg random)"/> </node></launch>

Resources:

Https://github.com/jihoonl/waypoint

Ros learns Python read-write text files