Wall (obstacle) following ROS Nodes

Question

I have mobile robot (just a two wheels squarish ) in gazebo and the robot is in the middle of a trapezoid wall.The robot has 5 sonar ultrasonic sensors, which measure distance from obstacles, as well as one an IMU (Inertial Measurement Unit) of 9 degrees freedom, which measures rotational speeds, linear accelerations, and rotation around each axis. The robot and dimensions are here

The starting orientation of the robot (rotation with respect to the z axis) which can be set in the launch file is:

angle = mod(X,π) (in rad)

2. I would like to creates ROS nodes that track (follow) the wall (obstacle) . In the nodes the direction of rotation (the direction in which the obstacle will be tracked) is determined. More specifically, if X is an even number, then the obstacle should be tracked clockwise (CW) with respect to the global coordinate system, while if it is odd the obstacle should be tracked anti-clockwise (CCW).

Here is the launch file with

<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
  <arg name="yaw_init" default="0.0"/>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
command="$(find xacro)/xacro '$(find mymobibot_description)/urdf/mymobibot.xacro'" />

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find mymobibot_gazebo)/worlds/mymobibot_wf.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mymobibot -Y $(arg yaw_init) -param robot_description"/>

  <!-- Run RVIZ-->
  <!--node name="$(anon rviz)" pkg="rviz" type="rviz" args="$(find mymobibot_gazebo)/mymobibot.rviz" output="screen"/-->

  <!-- ros_control mymobibot launch file -->
  <include file="$(find mymobibot_control)/launch/mymobibot_control.launch" />

</launch>

And the control launch

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find mymobibot_control)/config/mymobibot_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/mymobibot" args="joint_state_controller
 rightWheel_effort_controller
 leftWheel_effort_controller"/>


  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
    <param name="robot_description" command="$(find xacro)/xacro '$(find mymobibot_description)/urdf/mymobibot.xacro'" />
    <remap from="/joint_states" to="/mymobibot/joint_states" />
  </node>

</launch>

Any help with the obstacle (wall) following node?

Thanks