Control of UWSim vehicles with MATLAB through ipc_bridge

This post  describes how to interface control algorithms developed in Matlab with UWSim in the context of underwater simulation, using the IPC-Bridge.

IPC-Bridge is a ROS stack developed by Nathan Michael (Penn Robotics) that allows communication between Matlab and ROS via IPC. The current version of IPC-Bridges includes built-in support for nav_msgs/Odometry messages, which is the type of message that UWSim uses for updating the position and velocity of underwater vehicles.

You will need to create a new launch file that will set up the bridge for Odometry messages between Matlab and a ROS network:

<launch>  
  <node pkg="ipc" name="central" type="central" output="screen" 
        args="-su"></node>
  <node pkg="ipc_nav_msgs" name="example_node" 
        type="nav_msgs_Odometry_subscriber" output="screen">
    <remap from="~topic" to="/vehiclePose"/>
    <param name="message" value="odometry"/>  
  </node>
</launch>

You will also need to suitably set the ROS interface for vehicles in UWSim. Just place this code inside the part of your scene XML (replace the vehicle name by yours):

<ROSOdomToPAT> 
  <topic>/vehiclePose</topic>
  <vehicleName>vehicle</vehicleName>
</ROSOdomToPAT>

Then, you just have to publish an Odometry message from Matlab. You can take as template one of the examples that come with ipc_bridge. It should look something like this (which sets a sinusoidal trajectory around position 0,0,-3.5):

% create a publisher that publishes a geometry_msgs/Odometry message
pid_id=nav_msgs_Odometry('connect','publisher',
                         'example_module','odometry');

% create an empty geometry_msgs/Odometry message structure
msg=nav_msgs_Odometry('empty');
msg.header.frame_id='world';
msg.child_frame_id='vehicle';

angle=0.0;
while (1)    
  % Set position
  msg.pose.pose.position.x = 0.2*sin(2*angle);
  msg.pose.pose.position.y = 0.2*sin(angle);
  msg.pose.pose.position.z = -3.5+0.2*sin(angle);
  % Set orientation
  msg.pose.pose.orientation.x = 1;
  msg.pose.pose.orientation.y = 0;
  msg.pose.pose.orientation.z = 0;
  msg.pose.pose.orientation.w = 0;
  % Twist linear
  msg.twist.twist.linear.x = 0.0;
  msg.twist.twist.linear.y = 0.0;
  msg.twist.twist.linear.z = 0.0;
  % Twist angular
  msg.twist.twist.angular.x = 0.0;
  msg.twist.twist.angular.y = 0.0;
  msg.twist.twist.angular.z = 0.0;

  %Send message
  nav_msgs_Odometry('send',pid_id,msg);
  angle=angle+0.0001;
end
Advertisements
This entry was posted in Uncategorized and tagged , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s