md49_base_controller_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>                                                                            
#include <geometry_msgs/Twist.h>                                                                
#include <md49_serialport/md49_serialport.h>                                                              
#include <md49_messages/md49_data.h>                                                          
#include <md49_messages/md49_encoders.h>                                                      
#include "md49_base_controller/md49_base_controller_class.h"

int main( int argc, char* argv[] ){

    // *****************
    // * Init as ROS node *
    // *****************
    ros::init(argc, argv, "base_controller" );
    BaseController myBaseController;
    ros::Rate loop_rate(10);
    ROS_INFO("base_controller: base_controller running...");

    // *******************
    // * Open serialport *
    // *******************
    myBaseController.open_serialport();

    // *****************************
    // * Set initial MD49 settings *
    // *****************************
    myBaseController.init_md49(myBaseController.get_requested_speed_l(), myBaseController.get_requested_speed_r(), myBaseController.get_initial_md49_mode(), \
                               myBaseController.get_initial_md49_acceleration(), myBaseController.get_initial_md49_timeout(), myBaseController.get_initial_md49_regulator());

    // ************
    // * Mainloop *
    // ************
    while(myBaseController.n.ok())
    {
        // set speed on MD49 via UART as set through /cmd_vel if speed_l or speed_r changed since last cycle
        if ((myBaseController.get_requested_speed_l() != myBaseController.get_actual_speed_l()) || (myBaseController.get_requested_speed_r() != myBaseController.get_actual_speed_r()))
        {
            myBaseController.set_speed(myBaseController.get_requested_speed_l(),myBaseController.get_requested_speed_r());
            myBaseController.set_actual_speed_l(myBaseController.get_requested_speed_l());
            myBaseController.set_actual_speed_r(myBaseController.get_requested_speed_r());
            ROS_INFO("base_controller: Set speed_l=%i and speed_r=%i on MD49", myBaseController.get_requested_speed_l(), myBaseController.get_requested_speed_r());
        }
        // Read encoder- data from MD49 via UART and publish encoder values as read to topic /md49_encoders
        myBaseController.publish_encoders();
        // Read other- data from MD49 via UART and publish MD49 data as read to topic /md49_data
        myBaseController.publish_md49_data();
        // Loop
        ros::spinOnce();
        loop_rate.sleep();
    }// end.mainloop
    return 1;
} // end.main