.. _examples-rosturtle: ************ ROSTurtle.py ************ You can view and download this file on Github: `ROSTurtle.py `_ .. code-block:: python :linenos: #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # This is an EXUDYN python example # # Details: This example shows how to communicate between an exudyn simulation and ROS # To make use of this example, you need to # install ROS (ROS1 noetic) including rospy (see rosInterface.py) # prerequisite to use: # use a bash terminal to start the roscore with: # roscore # You can also use the ROS turtlesim_node to subsrcibe the Twist massage from exudyn # use a bash terminal to start the turtlesim node with: # rosrun turtlesim turtlesim_node turtle1/cmd_vel:=exudyn/Twist # Start example with argument -pub, to use it with external publisher: # python3 ROSTurtle.py -pub # Start example with argument -noTrack, to not track the turtle: # python3 ROSTurtle.py -noTrack # For even more ROS functionality create a ROS package (e.q. myExudynInterface) in a catkin workspace, # copy files ROSTurtle.py, Turtle.stl and ROSBringupTurtle.launch file (see folder Examples/supplementary) in corresponding folders within the package # For more functionality see also: ROSMassPoint.py, ROSBringupTurtle.launch, ROSControlTurtleVelocity.py # Author: Martin Sereinig, Peter Manzl # Date: 2023-05-31 (created) # # Copyright:This file is part of Exudyn. Exudyn is free software. # You can redistribute it and/or modify it under the terms of the Exudyn license. # See 'LICENSE.txt' for more details. # #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ import numpy as np import sys import exudyn as exu from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities import exudyn.graphics as graphics #only import if it does not conflict # import needed ROS modules and messages import rospy from geometry_msgs.msg import Twist from std_msgs.msg import String # import new exudyn ROS interface class import rosInterface as exuROS # here build inherited class and using within a simple exudyn simulation of one mass spring-damper class MyExudynROSInterface(exuROS.ROSInterface): def __init__(self): # use initialisation of super class # this initialize a rosnode with name super().__init__(name='ROSexampleTurtle') # initialization of all standard publisher done by super class # self.exuPosePublisher # self.exuStringPublisher # self.exuSystemstatePublisher # self.exuTimePublisher # self.exuTwistPublisher # use standard publisher functions form super class! # self.PublishPoseUpdate # self.PublishTwistUpdate # self.PublishSystemStateUpdate # initialize all subscriber # suitable callback function is auto generated by superclass (using lambda function) # twist subscriber: cmd_vel twistSubsrciberBase = '' twistSubsrciberTopic = 'cmd_vel' # topic to subscribe self.cmd_vel = Twist() # data type of topic, must be named: self.twistSubscriberTopic self.myTwistSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,Twist) # string subsriber: my_string stringSubsrciberBase = '' stringSubsrciberTopic = 'my_string' # topic to subscribe self.my_string = String() # data type of topic, must be named: self.twistSubscriberTopic self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase,stringSubsrciberTopic,String) def main(): #function to check if argument is in sys.argv with try/except def argumentInSysArgv(index): try: sys.argv[index] except IndexError: return '' else: return sys.argv[index] # turtle moves in circle and is tracked by default (no arguments) hearToPublisher = False saveTrack = True # parse command line arguments for multiple arguments: # -pub: use external publisher # -noTrack: do not track turtle if len(sys.argv) > 1: for arguments in range(len(sys.argv)): if argumentInSysArgv(arguments) == '-pub': hearToPublisher = True print('Wait for external ROS publisher on /cmd_vel for turtle to move') if argumentInSysArgv(arguments) == '-noTrack': saveTrack = False print('Turtle is not tracked') # build exudyn model SC = exu.SystemContainer() mbs = SC.AddSystem() tRes = 0.001 # step size in s tEnd = 1e5 # simulation time in s # density and dimension of box boxdensity = 1e-5 boxLength = [0.5, 0.25, 0.1] background = graphics.CheckerBoard(point=[0,0,0], normal=[0, 0, 1], color=[0.7]*3+[0.5], alternatingColor=[0.8]*3+[1], nTiles=10, size=10) graphicsCube = graphics.Brick(centerPoint = [0,0,0], size = boxLength, color = [1,0.,0.,1.], addNormals = False, addEdges = False, edgeColor = graphics.color.black, addFaces = True) inertiaCube = InertiaCuboid(density= boxdensity, sideLengths = boxLength) origin = [0, 0, 0] bGround = mbs.AddObject(ObjectGround(referencePosition=origin, visualization=VObjectGround(graphicsData=[background]))) # graphics userfunction definieren: if saveTrack: def graphicsTrajUF(mbs, itemNumber): t = mbs.systemData.GetTime(exu.ConfigurationType.Visualization) # position of turtle stored by sensor in mbs.variables['pos'] pOld = mbs.GetSensorStoredData(mbs.variables['pos']) try: iCurr = np.min([np.argmin(abs(pOld[:,0] - t)), len(pOld[:,0])-1]) pOld = pOld[:iCurr, :] except: pass if len(pOld) > 2: trajData = np.matrix.flatten(pOld[:,1:]).tolist() for i in range(int(len(trajData)/3)): trajData[2+3*i] += 0.115 # draw it on top of the robot graphicsTraj = {'type':'Line', 'data': trajData, 'color':graphics.color.blue} else: graphicsTraj = [] return [graphicsTraj] # add object ground with graphics user function to add turtle track oTrack = mbs.AddObject(ObjectGround(visualization =VObjectGround(graphicsData=[], graphicsDataUserFunction = graphicsTrajUF))) graphicsTurtleList = [] try: try: path2stl = rospy.get_param('/ROSExampleTurtle/stlFilePath') # node_name/argsname except: path2stl = '' print('stl file path: ', path2stl) turtleRot = RotationMatrixZ(-np.pi/2) stlGrafics = graphics.FromSTLfile(path2stl+'ROSTurtle.stl',color=[1,0,0,1],scale=0.25,pOff=[0.35,0,0], Aoff=turtleRot) graphicsTurtleList += [stlGrafics] except: print('stl not found, maybe wrong directory, use box instead') graphicsTurtleList += [graphicsCube] # user interaction point # old interface: # [nUIP, bUIP]=AddRigidBody (mainSys = mbs, # inertia = inertiaCube, # nodeType = str(exu.NodeType.RotationEulerParameters), # position = [origin[0], origin[1], origin[2]], # rotationMatrix = np.eye(3), # angularVelocity = np.array([0,0,0]), # velocity= [0,0,0], # gravity = [0, 0, 0], # graphicsDataList = graphicsTurtleList) dictUIP = mbs.CreateRigidBody( inertia=inertiaCube, referencePosition=[origin[0], origin[1], origin[2]], referenceRotationMatrix=np.eye(3), initialAngularVelocity=np.array([0, 0, 0]), initialVelocity=[0, 0, 0], gravity=[0, 0, 0], graphicsDataList=graphicsTurtleList, returnDict=True) [nUIP, bUIP] = [dictUIP['nodeNumber'], dictUIP['bodyNumber']] # create markers: mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0.0, 0.0, 0.0])) mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP)) dampingHelper = 1e-4 # create userfunction for TorsionalSpringDamper def UFtorque(mbs,t,itemNumber,r,av,k,d,offset): return (av-offset)*d # create TorsionalSpringDamper object oTSD = mbs.AddObject(TorsionalSpringDamper(markerNumbers = [mGround,mUIP], damping = dampingHelper, offset = 0, springTorqueUserFunction = UFtorque)) # create userfunction for CartesianSpringDamper def UFforce(mbs, t,itemNumber, u, v, k, d, offset): return [(v[0]-offset[0])*d[0], (v[1]-offset[1])*d[1], (v[2]-offset[2])*d[2]] # create CartesianSpringDamper object oCSD = mbs.AddObject(CartesianSpringDamper(markerNumbers = [mGround, mUIP], damping = [dampingHelper,dampingHelper,dampingHelper], offset = [0,0,0], springForceUserFunction = UFforce, visualization=VObjectConnectorCartesianSpringDamper(show=False))) sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.Position,storeInternal=True)) sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.Rotation)) sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.Velocity)) sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.AngularVelocity)) # store sensor value of each step in mbs variable, so that is accessible from user function mbs.variables['pos'] = sensorPos # just needed if sensor is used for sensor information mbs.variables['ori'] = sensorOri # just needed if sensor is used for sensor information mbs.variables['velt'] = sensorVelt # just needed if sensor is used for sensor information mbs.variables['velr'] = sensorVelr # just needed if sensor is used for sensor information mbs.variables['hearToPublisher'] = hearToPublisher # needed to use with and without external publisher mbs.variables['nodeNumber'] = nUIP # just needed if nodeNumber is used for sensor information # initialize ROS interface from own subclass myROSInterface = MyExudynROSInterface() print('rosversion: ' + str(myROSInterface.myROSversionEnvInt)) rospy.logdebug('node running and publishing') # exudyn PreStepUserFunction def PreStepUserFunction(mbs, t): # get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..." rosLinearVelo = myROSInterface.cmd_vel.linear rosAngularVelo = myROSInterface.cmd_vel.angular # EXAMPLE to get position and orientation from exudyn turtle via sensor turtlePosition = mbs.GetSensorValues(mbs.variables['pos']) turtleOrientation = mbs.GetSensorValues(mbs.variables['ori'])[2] turtleOrientationMatrix = RotationMatrixZ(turtleOrientation) # set velocities to exudyn turtle simulation if mbs.variables['hearToPublisher'] ==True: # exudyn turtle hears on publisher desiredLinearVelocity = turtleOrientationMatrix @ [rosLinearVelo.x, rosLinearVelo.y, rosLinearVelo.z] desiredAngularVelocity = [rosAngularVelo.x, rosAngularVelo.y, rosAngularVelo.z] else: # exudyn turtle moves in a circle desiredLinearVelocity = turtleOrientationMatrix @ [1, 0, 0] desiredAngularVelocity = [0, 0, 1] mbs.SetObjectParameter(oCSD, 'offset', desiredLinearVelocity) mbs.SetObjectParameter(oTSD, 'offset', desiredAngularVelocity[2]) # send velocity data to ROS myROSInterface.PublishTwistUpdate(mbs,t) # send position data to ROS myROSInterface.PublishPoseUpdate(mbs,t) # send system state data to ROS myROSInterface.PublishSystemStateUpdate(mbs,t) return True mbs.SetPreStepUserFunction(PreStepUserFunction) # assemble multi body system with all previous specified properties and components mbs.Assemble() # set simulation settings simulationSettings = exu.SimulationSettings() #takes currently set values or default values simulationSettings.timeIntegration.endTime = tEnd simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes) simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10 simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver simulationSettings.timeIntegration.newton.useModifiedNewton = False simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False simulationSettings.timeIntegration.simulateInRealtime = True # crucial for operating with robot simulationSettings.displayStatistics = True simulationSettings.solutionSettings.solutionInformation = "Exudyn-ROS turtle" simulationSettings.solutionSettings.writeSolutionToFile = False SC.visualizationSettings.general.autoFitScene = True # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10 SC.visualizationSettings.interactive.trackMarker = mUIP SC.visualizationSettings.window.startupTimeout = 8000 SC.visualizationSettings.openGL.initialZoom = 0.2 SC.visualizationSettings.openGL.initialMaxSceneSize= 0.7 SC.visualizationSettings.interactive.selectionLeftMouse=False SC.visualizationSettings.interactive.selectionRightMouse=False exu.StartRenderer(True) exu.SolveDynamic(mbs, simulationSettings) return True # create a function # __main__ function if __name__ == "__main__": try: main() except rospy.ROSInterruptException: pass