As in all classes the member variables should be initialized. Component pointers need access to an RTL component handler that can create some types of components and refer to existing ones, a global list where this component will be registered so other pointers refer to it, and the parent component (this) for a clear hierarchy. For the numerical members you should choose reasonable values - they will be used when no values are specified in the simulation file.

            TESTEXTENSION::RCFollowWallController::RCFollowWallController(
                    const RCString&              sKey,
                    const RSControllerCtorArg&   CtorArg)    :
                TRCControllerComponent<RCFollowWallController>(
                        *this,
                        sKey,
                        CtorArg),
                m_LeftSensorPtr     (m_RtlComponentHandler, m_pGlobalList, this),
                m_RightSensorPtr    (m_RtlComponentHandler, m_pGlobalList, this),
                m_MotionControlPtr  (m_RtlComponentHandler, m_pGlobalList, this),
                m_nLinVelocity(0.2),
                m_nRotVelocity(0.5),
                m_nPeriod(0.1),
                m_ThreadPtr(CreateThread())
		

Realize(): a kind of initializatin function called when the simulation is loaded.
Adapt the value of sOwner (it is displayed in error messaged when the Realization of this component fails):

            const RCString sOwner = 
                RCString(_T("follow wall controller'")) + GetFullComponentKey() + _T("'");
		

Add the realization of all member components (the specifies strings are only used to produce meaningful error messages):

            bSuccess &=  m_LeftSensorPtr    .Realize(_T("left sensor"),    sOwner);
            bSuccess &=  m_RightSensorPtr   .Realize(_T("right sensor"),   sOwner);
            bSuccess &=  m_MotionControlPtr .Realize(_T("motion control"), sOwner);
		

Clear(): called when the simulation is about to be closed. Since the component pointers hold a smart pointer internally components are only destroyed when no more pointer to it exists. Since it is possible to create cyclic dependencies (in the simulation file you can also refer to components created later in that file) the instances in such a cycle would never be destroyed and cause a memory leak. Thus, we better remove all links before destruction here:

            void TESTEXTENSION::RCFollowWallController::Clear()
            {
                m_LeftSensorPtr.Clear();
                m_RightSensorPtr.Clear();
                m_MotionControlPtr.Clear();
            
                TRCControllerComponent<RCFollowWallController>::Clear();
            }
		

ThreadFcn(void*): The function running in an extra thread.
No initialization necessary for our controller.
Since the -> operator of TRCRtlComponentPointer is overloaded you can access the functions of the corresponding interface directly. So the implementation of the algorithm is quite straight forward:

            TLong TESTEXTENSION::RCFollowWallController::ThreadFcn(void*)
            {
                while (!QuitEvent().IsSet())
                {
                    if (m_LeftSensorPtr->CheckContact())
                    {
                        m_MotionControlPtr->SetDestAngVelocity(m_nRotVelocity);
                        m_MotionControlPtr->SetDestLinVelocity(0);
                    }
                    else if (m_RightSensorPtr->CheckContact())
                    {
                        m_MotionControlPtr->SetDestAngVelocity(-m_nRotVelocity);
                        m_MotionControlPtr->SetDestLinVelocity(0);
                    }
                    else
                    {
                        m_MotionControlPtr->SetDestAngVelocity(0);
                        m_MotionControlPtr->SetDestLinVelocity(m_nLinVelocity);
                    }
            
                    Sleep(Round(m_nPeriod * MILLI_INV));
                }
                return 0;
            }
		

Now we are done with our controller and can compile it (and fix all the typos we made). If it was successfull and we start the simulator now, it should have loaded the extension with our new controller automatically.

Step 4: Create a Simulation with the new Controller

To see our controller in action we need to create a simulation that uses it. It is quite a lot work to wire a simulationfile from scratch so we better take an existing one and adapt it - inthis case we take SimpleRemoteControlledRobot.
First we copy the xml file (the ini file is not necessary) and rename it.
In the simulation file (I can recommend notepad++ to edit those files) we have to remove the RCSimpleRobotComControl and the RCLocalSocketCommunicator entry since this components are no not required for our simulation.
So far we have a simulation with bounding walls (and ground and sky) and a robot with two driven wheels and a balancing ball. The next step is to add the proximity-sensors:
They are peripheries of the robot so we need to add them in the ChildList (line 184 right behind the RCBallSocketJoint). Here we add the description:

            <RCProximitySensor Key="LeftSensor">
                <Pose>
                    <RCLocalCoordinates>
                        <Pose Value="(-0.22, 0, 0.12)(0, 0, 0)"/>
                    </RCLocalCoordinates>
                </Pose>
                <Geom>
                    <RCSphereGeom>
                        <Radius Value="0.02"/>
                    </RCSphereGeom>
                </Geom>
            </RCProximitySensor>
            <RCProximitySensor Key="RightSensor">
                <Pose>
                    <RCLocalCoordinates>
                        <Pose Value="(-0.22, 0, -0.12)(0, 0, 0)"/>
                    </RCLocalCoordinates>
                </Pose>
                <Geom>
                    <RCSphereGeom>
                        <Radius Value="0.02"/>
                    </RCSphereGeom>
                </Geom>
            </RCProximitySensor>
		

The sensor detecs a contact when it's correspoinding geometry collides with any other object - so make sure you don't place it too close to the robot so that it causes already a contact itself. The code above will produce two (invisible) spheres with a radius of 2cm beeing placed at the front corners of the robots body with sufficient distance.
Now we can add our own controller in the robots controller list and fill in the parameters:
For the two sensors we need to refer to the proximity sensors declared above. Since their key is unique in this simulation there's no need to add also the parents key in front of it.
To specify the parameter MotionControl we need a controller that implements the interface RC2DMotionControl and that is suitable for our robot contruction. The component RCTwoWheelMotionControl fits this need. We could either add the controller parallel to our RCFollowWallController and refer to it or create it in-place (as we do it here now).
Since the default-values for the other parameters (LinVelocity, RotVelocity and Period) are reasonable we could drop them here but it's always nice to specify all possible parameters so you can easily see what can be adapted in the simulation. The complete description of our controller looks like this:

            <RCFollowWallController>
                <LeftSensor Ref="LeftSensor"/>
                <RightSensor Ref="RightSensor"/>
                <MotionControl>
                    <RCTwoWheelMotionControl>
                        <ControlLeft Ref="LeftDrive"/>
                        <ControlRight Ref="RightDrive"/>
                        <WheelDistance Value="0.22"/>
                        <WheelRadius Value="0.03"/>
                    </RCTwoWheelMotionControl>
                </MotionControl>
                <LinVelocity Value="0.2"/>
                <RotVelocity Value="0.5"/>
                <Period Value="0.1"/>
            </RCFollowWallController>
		

Running the simulation we will see that the robot hits the wall when rotating - a result of the badly places sensors. Changing their X-coordinates to -0.22 instaed of -0.12 (just change the simulation file and click on the reload-button) will fix this problem and result in a controller nicely following the walls.

Download!