Accelerate Robotics Algorithms with Code Generation

You can generate code for select Robotics System Toolbox™ algorithms to speed up their execution. Set up the algorithm that supports code generation as a separate function that you can insert into your workflow. To use code generation, you must have a MATLAB® Coder™ license. For a summary of code generation support in Robotics System Toolbox, see Code Generation.

For this example, a robot is wandering in an environment using the VectorFieldHistogram class with laser scan data to perform obstacle avoidance. The goal is to replace the vector field histogram (VFH) algorithm with a MEX file created from code generation.

To see this example without code generation, see Obstacle Avoidance with TurtleBot and VFH.

Create Separate Function for Algorithm

Create a separate function, vfhCodeGen, that runs the vector field algorithm. Create a VFH object and specify the algorithm parameters. Call the object as the main function to use the VFH algorithm. Specify %#codegen inside the function to identify it as a function for code generation.

function steerDir = vfhCodeGen(ranges,angles,targetDir)
	%#codegen	
	vfh = robotics.VectorFieldHistogram;
	vfh.DistanceLimits = [0.05 1];
	vfh.RobotRadius = 0.1;
	vfh.MinTurningRadius = 0.2;
	vfh.SafetyDistance = 0.1;
	
	steerDir = vfh(ranges,angles,targetDir); 
end 

Save the function in your current folder.

Perform Code Generation for Algorithm

You can use either the codegen function or the MATLAB Coder app to generate code. In this example, you generate a MEX file by callingcodegen on the MATLAB command line. Specify sample input arguments for each input to the function using the -args input argument

Specify sample values for the input arguments. Create a sample of the ranges, angles, and target directions. The TurtleBot® laser scan gives 640 scans.

ranges = zeros(640,1);
angles = zeros(640,1);
targetDir = 0;

Call the codegen function and specify the input arguments in a cell array. This function creates a separate vfhCodeGen_mex function to use. You can also produce C code by using the options input argument.

codegen vfhCodeGen -args {ranges,angles,targetDir}

If your laser scan can come from different sources with variable-size lengths, specify the canonical type of the ranges and angles inputs by using coder.typeof with the codegen function.

codegen vfhCodeGen -args {coder.typeof(ranges,[Inf 1]), ...
                          coder.typeof(angles,[Inf 1]),targetDir}

Check Performance of Generated Code

Compare the timing of the generated MEX function to the timing of your original function by using timeit.

time = timeit(@() vfhCodeGen(ranges,angles,targetDir))
mexTime = timeit(@() vfhCodeGen_mex(ranges,angles,targetDir))
time =

    0.0039


mexTime =

   7.6490e-05

The MEX function runs over 50 times faster in this example. Results might vary in your system.

Replace Algorithm Function with MEX Function

Open the main function for running your robotics workflow. Replace the vfh object call with the MEX function that you created using code generation.

Open the Obstacle Avoidance with TurtleBot and VFH example.

openExample('robotics/ObstacleAvoidanceWithTurtleBotAndVFHExample')

Modify the example code to use the new vfhCodeGen_mex function. The code that follows is a copy of the example with modified comments and use of the new MEX function. The definition of the VFH object is also removed.

Connect to the TurtleBot. Set up the laser scan subscriber, the velocity publisher, and the rate control object. Specify a starting target direction.

rosinit('192.168.154.131')
laserSub = rossubscriber('/scan');
[velPub, velMsg] = rospublisher('/mobile_base/commands/velocity');
rate = robotics.Rate(10);
targetDir = 0;

Create a loop that collects data, calculates the steering direction, and drives the robot. Set a loop time of 30 seconds. Replace the step call with vfhCodeGen_mex.

while rate.TotalElapsedTime < 30

	% Get laser scan data
	laserScan = receive(laserSub);
	ranges = double(laserScan.Ranges);
	angles = double(readScanAngles(laserScan));
	
	% Call MEX function created using code generation
	steerDir = vfhCodeGen_mex(ranges,angles,targetDir);
    
	% Calculate velocities
	if ~isnan(steerDir) % If steering direction is valid
		desiredV = 0.2;
		w = exampleHelperComputeAngularVelocity(steerDir,1);
	else % Stop and search for valid direction
		desiredV = 0.0;
		w = 0.5;
	end

	% Assign and send velocity commands
	velMsg.Linear.X = desiredV;
	velMsg.Angular.Z = w;
	send(velPub,velMsg);
end

The robot performs 30 seconds of obstacle avoidance using the MEX function. For this application, the time difference of the VFH algorithm is minimal, but you can use this example to help you replace other algorithms with generated code. Consider using code generation in areas of your code that slow down the workflow.

Disconnect from the ROS network.

rosshutdown

See Also

| |

Related Topics