// Listen for Pltf Cmds
		void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg)
		{
			double vx_cmd_mms, vy_cmd_mms, w_cmd_rads;
			
			// controller expects velocities in mm/s, ROS works with SI-Units -> convert
			// ToDo: rework Controller Class to work with SI-Units
			vx_cmd_mms = msg->linear.x*1000.0;
			vy_cmd_mms = msg->linear.y*1000.0;
			w_cmd_rads = msg->angular.z;

			// only process if controller is already initialized
			if (is_initialized_bool_)
            {
				ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]", 
                     msg->linear.x, msg->linear.y, msg->angular.z);

				// Set desired value for Plattform Velocity to UndercarriageCtrl (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity(vx_cmd_mms, vy_cmd_mms, w_cmd_rads, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
			}
			else
			{	
				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
			}
		}

		// Listen for Emergency Stop
		void topicCallbackEMStop(const cob_msgs::EmergencyStopState::ConstPtr& msg)
		{
			int EM_state;
			EM_state = msg->emergency_state;

			if (EM_state == msg->EMFREE)
			{
				// Reset EM flag in Ctrlr
				if (is_initialized_bool_) 
				{
					ucar_ctrl_.setEMStopActive(false);
					// reset only done, when system initialized
					// -> allows to stop ctrlr during init, reset and shutdown
				}
			}
			else
			{
            	ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop");

				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
		
				// Set EM flag and stop Ctrlr
				ucar_ctrl_.setEMStopActive(true);
			}
		}

		// Listens for status of underlying hardware (base drive chain)
		void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
		{
			sensor_msgs::JointState joint_state_cmd;

			// prepare joint_cmds for heartbeat (compose header)
			joint_state_cmd.header.stamp = ros::Time::now();
			//joint_state_cmd.header.frame_id = frame_id; //Where to get this id from?
			// ToDo: configure over Config-File (number of motors) and Msg
			// assign right size to JointState data containers
			//joint_state_cmd.set_name_size(m_iNumMotors);
			joint_state_cmd.position.resize(m_iNumJoints);
			joint_state_cmd.velocity.resize(m_iNumJoints);            
			joint_state_cmd.effort.resize(m_iNumJoints);
			// compose jointcmds
			for(int i=0; i<m_iNumJoints; i++)
			{					
				joint_state_cmd.position[i] = 0.0;
				joint_state_cmd.velocity[i] = 0.0;
				joint_state_cmd.effort[i] = 0.0;
			}
			
			// set status of underlying drive chain to member variable 
			drive_chain_diagnostic_ = msg->level;

			// if controller is already started up ...
			if (is_initialized_bool_)
			{
				// ... but underlying drive chain is not yet operating normal
				if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
				{
					// halt controller
            		ROS_DEBUG("drive chain not availlable: halt Controller");

					// Set EM flag to Ctrlr (resets internal states)
					ucar_ctrl_->setEMStopActive(true);

					// Set desired value for Plattform Velocity to zero (setpoint setting)
					ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
					// ToDo: last value (0.0) is not used anymore --> remove from interface
					ROS_DEBUG("Forced platform-velocity cmds to zero");
					
					// if is not Initializing
					if (drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
					{
						// publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr
						topic_pub_joint_state_cmd_.publish(joint_state_cmd);
					}
				}
			}
			// ... while controller is not initialized send heartbeats to keep motors alive
			else
			{
				// ... as soon as base drive chain is initialized
				if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
				{
					// publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr
					topic_pub_joint_state_cmd_.publish(joint_state_cmd);
				}
			}
		}

// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred)
void NodeClass::CalcCtrlStep()
{
	double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy;
	std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad;
	sensor_msgs::JointState joint_state_cmd;
	int num_joints = 8;
	int j, k;
	
	// if controller is initialized and underlying hardware is operating normal
	if (is_initialized_bool_ && (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK))
	{
		// perform one control step,
		// get the resulting cmd's for the wheel velocities and -angles from the controller class
		// and output the achievable pltf velocity-cmds (if velocity limits where exceeded)
		ucar_ctrl_.GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads,  steer_jointvel_cmds_rads, 									steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy);
		// ToDo: adapt interface of controller class --> remove last values (not used anymore)

		// convert variables to SI-Units
		vx_cmd_ms = vx_cmd_ms/1000.0;
		vy_cmd_ms = vy_cmd_ms/1000.0;

		// compose jointcmds
		// compose header
		joint_state_cmd.header.stamp = ros::Time::now();
		//joint_state_cmd.header.frame_id = frame_id; //Where to get this id from?
		// ToDo: configure over Config-File (number of motors) and Msg
		// assign right size to JointState data containers
		//joint_state_cmd.set_name_size(m_iNumMotors);
		joint_state_cmd.set_position_size(num_joints);
		joint_state_cmd.set_velocity_size(num_joints);            
		joint_state_cmd.set_effort_size(num_joints);

		// compose data body
		j = 0;
		k = 0;
		for(int i = 0; i<num_joints; i++)
		{
			// for steering motors
			if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the Msg
			{
				joint_state_cmd.position[i] = steer_jointang_cmds_rad[j];
				joint_state_cmd.velocity[i] = steer_jointvel_cmds_rads[j];
				joint_state_cmd.effort[i] = 0.0;
				j = j + 1;
			}
			else
			{
				joint_state_cmd.position[i] = 0.0;
				joint_state_cmd.velocity[i] = drive_jointvel_cmds_rads[k];
				joint_state_cmd.effort[i] = 0.0;
				k = k + 1;
			}
		}

		// publish jointcmds
		topic_pub_joint_state_cmd_.publish(joint_state_cmd);
	}

}

// acquires the current undercarriage configuration from base_drive_chain
void NodeClass::GetJointState()
{
	int num_joints;
	int iter_k, iter_j;
	std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM;
	std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM;
	cob_srvs::GetJointState srv_get_joint;
	
	// request update for pltf config --> call GetJointstate service
	srv_client_get_joint_state_.call(srv_get_joint);

	// copy configuration into vector classes
	num_joints = srv_get_joint.response.jointstate.position.size();
	// drive joints
	drive_joint_ang_rad.assign(num_joints, 0.0);
	drive_joint_vel_rads.assign(num_joints, 0.0);
	drive_joint_effort_NM.assign(num_joints, 0.0);
	// steer joints
	steer_joint_ang_rad.assign(num_joints, 0.0);
	steer_joint_vel_rads.assign(num_joints, 0.0);
	steer_joint_effort_NM.assign(num_joints, 0.0);

	// init iterators
	iter_k = 0;
	iter_j = 0;

	for(int i = 0; i < num_joints; i++)
	{
		// associate inputs to according steer and drive joints
		// ToDo: specify this globally (Prms-File or config-File or via msg-def.)
		// ToDo: use joint names instead of magic integers
		if( i == 1 || i == 3 || i == 5 || i == 7)
		{
			steer_joint_ang_rad[iter_k] = srv_get_joint.response.jointstate.position[i];
			steer_joint_vel_rads[iter_k] = srv_get_joint.response.jointstate.velocity[i];
			steer_joint_effort_NM[iter_k] = srv_get_joint.response.jointstate.effort[i];
			iter_k = iter_k + 1;
		}
		else
		{
			drive_joint_ang_rad[iter_j] = srv_get_joint.response.jointstate.position[i];
			drive_joint_vel_rads[iter_j] = srv_get_joint.response.jointstate.velocity[i];
			drive_joint_effort_NM[iter_j] = srv_get_joint.response.jointstate.effort[i];
			iter_j = iter_j + 1;
		}
	}

	// Set measured Wheel Velocities and Angles to Controler Class (implements inverse kinematic)
	ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads,
							drive_joint_ang_rad, steer_joint_ang_rad);
}

		// Listens for status of underlying hardware (base drive chain)
		void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
		{
			// set status of underlying drive chain to member variable 
			drive_chain_diagnostic_ = msg->level;

			// if controller is already started up ...
			if (is_initialized_bool_)
			{
				// ... but underlying drive chain is not yet operating normal
				if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
				{
					// halt controller
            		ROS_DEBUG("drive chain not availlable: halt Controller");

					// Set EM flag to Ctrlr (resets internal states)
					ucar_ctrl_.setEMStopActive(true);

					// Set desired value for Plattform Velocity to zero (setpoint setting)
					ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
					// ToDo: last value (0.0) is not used anymore --> remove from interface
				}
			}
		}

//##################################
//#### function implementations ####
bool NodeClass::InitCtrl()
{
    ROS_INFO("Initializing Undercarriage Controller");

    // ToDo:
	// 1st step: pass the path to the Inifile via the Parameterserver
	// 2nd step: replace the Inifiles by ROS configuration files
    
	// create Inifile class and set target inifile (from which data shall be read)
	//IniFile iniFile;
	// Parameters are set within the launch file
	//n.param<std::string>("base_drive_chain_node/IniDirectory", sIniDirectory, "Platform/IniFiles/");
	//ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
	//iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");

	// Init Controller Class
	ucar_ctrl_.InitUndercarriageCtrl();
	ROS_INFO("Initializing Undercarriage Controller done");

    return true;
}

		// shutdown Controller
        bool srvCallbackShutdown(cob_srvs::Switch::Request &req,
                                     cob_srvs::Switch::Response &res )
        {
			if (is_initialized_bool_)
			{
				// stop controller
				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);

				// flag that controller is not running anymore
				is_initialized_bool_ = false;
	
				res.success = true;
			}
			else
			{
				// shutdown not possible, because pltf not running
                ROS_ERROR("...platform not initialized...");
                res.success = false;
                res.errorMessage.data = "platform already or still down";
			}
	    	return true;
        }

// calculates odometry from current measurement values
// and publishes it via an odometry topic and the tf broadcaster
void NodeClass::UpdateOdometry()
{
	double vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
	double dummy1, dummy2;
	double dt;
	ros::Time current_time;

	// if drive chain already initialized process joint data
	if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
	{
		// Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics)
		// !Careful! Controller internally calculates with mm instead of m
		// ToDo: change internal calculation to SI-Units
		// ToDo: last values are not used anymore --> remove from interface
		ucar_ctrl_.GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
									vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);

		// convert variables to SI-Units
		vel_x_rob_ms = vel_x_rob_ms/1000.0;
		vel_y_rob_ms = vel_y_rob_ms/1000.0;
		delta_x_rob_m = delta_x_rob_m/1000.0;
		delta_y_rob_m = delta_y_rob_m/1000.0;
	}
	else
	{
		// otherwise set data (velocity and pose-delta) to zero
		vel_x_rob_ms = 0.0;
		vel_y_rob_ms = 0.0;
		delta_x_rob_m = 0.0;
		delta_y_rob_m = 0.0;
	}

	// calc odometry (from startup)
	// get time since last odometry-measurement
	current_time = ros::Time::now();
	dt = current_time.toSec() - last_time_.toSec();
	last_time_ = current_time;
	// calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom
	x_rob_m_ = x_rob_m_ + (vel_x_rob_ms * cos(theta_rob_rad_) - vel_y_rob_ms * sin(theta_rob_rad_)) * dt;
	y_rob_m_ = y_rob_m_ + (vel_x_rob_ms * sin(theta_rob_rad_) + vel_y_rob_ms * cos(theta_rob_rad_)) * dt;
	theta_rob_rad_ = rot_rob_rads * dt;

	// format data for compatibility with tf-package and standard odometry msg
	// generate quaternion for rotation
	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);

	// compose and publish transform for tf package
	geometry_msgs::TransformStamped odom_tf;
	// compose header
	odom_tf.header.stamp = current_time;
	odom_tf.header.frame_id = "/odom";
	odom_tf.child_frame_id = "/base_footprint";
	// compose data container
	odom_tf.transform.translation.x = x_rob_m_;
	odom_tf.transform.translation.y = y_rob_m_;
	odom_tf.transform.translation.z = 0.0;
	odom_tf.transform.rotation = odom_quat;

    // compose and publish odometry message as topic
    nav_msgs::Odometry odom_top;
	// compose header
    odom_top.header.stamp = current_time;
    odom_top.header.frame_id = "/odom";
    odom_top.child_frame_id = "/base_footprint";
    // compose pose of robot
    odom_top.pose.pose.position.x = x_rob_m_;
    odom_top.pose.pose.position.y = y_rob_m_;
    odom_top.pose.pose.position.z = 0.0;
    odom_top.pose.pose.orientation = odom_quat;
    // compose twist of robot
    odom_top.twist.twist.linear.x = vel_x_rob_ms;
    odom_top.twist.twist.linear.y = vel_y_rob_ms;
    odom_top.twist.twist.linear.z = 0.0;
    odom_top.twist.twist.angular.x = 0.0;
    odom_top.twist.twist.angular.y = 0.0;
    odom_top.twist.twist.angular.z = rot_rob_rads;

	// publish data
	// publish the transform
	tf_broadcast_odometry_.sendTransform(odom_tf);
	// publish odometry msg
	topic_pub_odometry_.publish(odom_top);
}

		// reset Controller Configuration
        bool srvCallbackReset(cob_srvs::Switch::Request &req, cob_srvs::Switch::Response &res )
        {
			bool ctrlr_reset;

			if (is_initialized_bool_)
			{

				// flag that Controller is not initialized
				is_initialized_bool_ = false;

				// first of all stop controller (similar to EMStop)
				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
				// Set EM flag and stop Ctrlr
				ucar_ctrl_.setEMStopActive(true);

				// now re-init controller configuration
				ctrlr_reset = InitCtrl();

				if (ctrlr_reset)
				{
					// restart controller
					// Set desired value for Plattform Velocity to zero (setpoint setting)
					ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
					// ToDo: last value (0.0) is not used anymore --> remove from interface
					// Set EM flag and stop Ctrlr
					ucar_ctrl_.setEMStopActive(false);

					// reset Time
					last_time_ = ros::Time::now();
					// and reset odometry
					x_rob_m_ = 0.0;
					y_rob_m_ = 0.0;
					theta_rob_rad_ = 0.0;

					// flag that controller is initialized
					is_initialized_bool_ = true;

					// set answer for service request
	       	    	ROS_INFO("Undercarriage Controller resetted");
					res.success = true;
				}
				else
				{
					// set answer for service request
	       	    	ROS_INFO("Re-Init after Reset of Undercarriage Controller failed");
					res.success = false;
                	res.errorMessage.data = "reinit after reset of undercarriage controller failed";
				}
			}
			else
			{
				// Reset not possible, because Controller not yet set up
                ROS_ERROR("... undercarriage controller not yet initialized, reset not possible ...");
				// set answer for service request
                res.success = false;
                res.errorMessage.data = "undercarriage controller not yet initialized";
			}

		    return true;
        }

// calculates odometry from current measurement values
// and publishes it via an odometry topic and the tf broadcaster
void NodeClass::UpdateOdometry()
{
    double vel_x_rob_ms, vel_y_rob_ms, vel_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
    double dummy1, dummy2;
    double dt;
    ros::Time current_time;

    // if drive chain already initialized process joint data
    //if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
    if (is_initialized_bool_)
    {
        // Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics)
        // !Careful! Controller internally calculates with mm instead of m
        // ToDo: change internal calculation to SI-Units
        // ToDo: last values are not used anymore --> remove from interface
        ucar_ctrl_->GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
                                          vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);

        // convert variables to SI-Units
        vel_x_rob_ms = vel_x_rob_ms/1000.0;
        vel_y_rob_ms = vel_y_rob_ms/1000.0;
        delta_x_rob_m = delta_x_rob_m/1000.0;
        delta_y_rob_m = delta_y_rob_m/1000.0;

        ROS_DEBUG("Odmonetry delta is: x=%f, y=%f, th=%f", delta_x_rob_m, delta_y_rob_m, rot_rob_rads);
    }
    else
    {
        // otherwise set data (velocity and pose-delta) to zero
        vel_x_rob_ms = 0.0;
        vel_y_rob_ms = 0.0;
        delta_x_rob_m = 0.0;
        delta_y_rob_m = 0.0;
    }

    // calc odometry (from startup)
    // get time since last odometry-measurement
    current_time = ros::Time::now();
    dt = current_time.toSec() - last_time_.toSec();
    last_time_ = current_time;
    vel_rob_ms = sqrt(vel_x_rob_ms*vel_x_rob_ms + vel_y_rob_ms*vel_y_rob_ms);

    // calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom, using now midpoint integration
    x_rob_m_ = x_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * cos(theta_rob_rad_) - (vel_y_rob_ms+vel_y_rob_last_)/2.0 * sin(theta_rob_rad_)) * dt;
    y_rob_m_ = y_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * sin(theta_rob_rad_) + (vel_y_rob_ms+vel_y_rob_last_)/2.0 * cos(theta_rob_rad_)) * dt;
    theta_rob_rad_ = theta_rob_rad_ + rot_rob_rads * dt;
    //theta_rob_rad_ = theta_rob_rad_ + (rot_rob_rads+vel_theta_rob_last_)/2.0 * dt;

    vel_x_rob_last_ = vel_x_rob_ms;
    vel_y_rob_last_ = vel_y_rob_ms;
    vel_theta_rob_last_ = rot_rob_rads;


    // format data for compatibility with tf-package and standard odometry msg
    // generate quaternion for rotation
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);

    // compose and publish transform for tf package
    geometry_msgs::TransformStamped odom_tf;
    // compose header
    odom_tf.header.stamp = joint_state_odom_stamp_;
    odom_tf.header.frame_id = "/wheelodom";
    odom_tf.child_frame_id = "/base_footprint";
    // compose data container
    odom_tf.transform.translation.x = x_rob_m_;
    odom_tf.transform.translation.y = y_rob_m_;
    odom_tf.transform.translation.z = 0.0;
    odom_tf.transform.rotation = odom_quat;

    // compose and publish odometry message as topic
    nav_msgs::Odometry odom_top;
    // compose header
    odom_top.header.stamp = joint_state_odom_stamp_;
    odom_top.header.frame_id = "/wheelodom";
    odom_top.child_frame_id = "/base_footprint";
    // compose pose of robot
    odom_top.pose.pose.position.x = x_rob_m_;
    odom_top.pose.pose.position.y = y_rob_m_;
    odom_top.pose.pose.position.z = 0.0;
    odom_top.pose.pose.orientation = odom_quat;
    for(int i = 0; i < 6; i++)
        odom_top.pose.covariance[i*6+i] = 0.1;

    // compose twist of robot
    odom_top.twist.twist.linear.x = vel_x_rob_ms;
    odom_top.twist.twist.linear.y = vel_y_rob_ms;
    odom_top.twist.twist.linear.z = 0.0;
    odom_top.twist.twist.angular.x = 0.0;
    odom_top.twist.twist.angular.y = 0.0;
    odom_top.twist.twist.angular.z = rot_rob_rads;
    for(int i = 0; i < 6; i++)
        odom_top.twist.covariance[6*i+i] = 0.1;

    if (fabs(vel_x_rob_ms) > 0.005 or fabs(vel_y_rob_ms) > 0.005 or fabs(rot_rob_rads) > 0.005)
    {
        is_moving_ = true;
    }
    else
//.........这里部分代码省略.........

// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred)
void NodeClass::CalcCtrlStep()
{
    double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy;
    std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad;
    pr2_controllers_msgs::JointTrajectoryControllerState joint_state_cmd;
    int j, k;
    iwatchdog_ += 1;

    // if controller is initialized and underlying hardware is operating normal
    if (is_initialized_bool_) //&& (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK))
    {
        // as soon as (but only as soon as) platform drive chain is initialized start to send velocity commands
        // Note: topicCallbackDiagnostic checks whether drives are operating nominal.
        //       -> if warning or errors are issued target velocity is set to zero

        // perform one control step,
        // get the resulting cmd's for the wheel velocities and -angles from the controller class
        // and output the achievable pltf velocity-cmds (if velocity limits where exceeded)
        ucar_ctrl_->GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads,  steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy);
        // ToDo: adapt interface of controller class --> remove last values (not used anymore)

        // if drives not operating nominal -> force commands to zero
        if(drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
        {
            steer_jointang_cmds_rad.assign(m_iNumJoints, 0.0);
            steer_jointvel_cmds_rads.assign(m_iNumJoints, 0.0);
        }

        // convert variables to SI-Units
        vx_cmd_ms = vx_cmd_ms/1000.0;
        vy_cmd_ms = vy_cmd_ms/1000.0;

        // compose jointcmds
        // compose header
        joint_state_cmd.header.stamp = ros::Time::now();
        //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from?
        // ToDo: configure over Config-File (number of motors) and Msg
        // assign right size to JointState data containers
        //joint_state_cmd.set_name_size(m_iNumMotors);
        joint_state_cmd.desired.positions.resize(m_iNumJoints);
        joint_state_cmd.desired.velocities.resize(m_iNumJoints);
        //joint_state_cmd.effort.resize(m_iNumJoints);
        joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint");
        joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint");
        joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint");
        joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint");
        joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint");
        joint_state_cmd.joint_names.push_back("br_caster_rotation_joint");
        joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint");
        joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint");

        // compose data body
        j = 0;
        k = 0;
        for(int i = 0; i<m_iNumJoints; i++)
        {
            if(iwatchdog_ < 50)
            {
                // for steering motors
                if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the Msg
                {
                    joint_state_cmd.desired.positions[i] = steer_jointang_cmds_rad[j];
                    joint_state_cmd.desired.velocities[i] = steer_jointvel_cmds_rads[j];
                    //joint_state_cmd.effort[i] = 0.0;
                    j = j + 1;
                }
                else
                {
                    joint_state_cmd.desired.positions[i] = 0.0;
                    joint_state_cmd.desired.velocities[i] = drive_jointvel_cmds_rads[k];
                    //joint_state_cmd.effort[i] = 0.0;
                    k = k + 1;
                }
            }
            else
            {
                joint_state_cmd.desired.positions[i] = 0.0;
                joint_state_cmd.desired.velocities[i] = 0.0;
                //joint_state_cmd.effort[i] = 0.0;
            }
        }

        // publish jointcmds
        topic_pub_controller_joint_command_.publish(joint_state_cmd);
    }

}

    void topicCallbackJointControllerStates(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg) {
        int num_joints;
        int iter_k, iter_j;
        std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM;
        std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM;
        cob_base_drive_chain::GetJointState srv_get_joint;

        joint_state_odom_stamp_ = msg->header.stamp;

        // copy configuration into vector classes
        num_joints = msg->joint_names.size();
        // drive joints
        drive_joint_ang_rad.assign(m_iNumJoints, 0.0);
        drive_joint_vel_rads.assign(m_iNumJoints, 0.0);
        drive_joint_effort_NM.assign(m_iNumJoints, 0.0);
        // steer joints
        steer_joint_ang_rad.assign(m_iNumJoints, 0.0);
        steer_joint_vel_rads.assign(m_iNumJoints, 0.0);
        steer_joint_effort_NM.assign(m_iNumJoints, 0.0);

        // init iterators
        iter_k = 0;
        iter_j = 0;

        for(int i = 0; i < num_joints; i++)
        {
            // associate inputs to according steer and drive joints
            // ToDo: specify this globally (Prms-File or config-File or via msg-def.)
            if(msg->joint_names[i] ==  "fl_caster_r_wheel_joint")
            {
                drive_joint_ang_rad[0] = msg->actual.positions[i];
                drive_joint_vel_rads[0] = msg->actual.velocities[i];
                //drive_joint_effort_NM[0] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "bl_caster_r_wheel_joint")
            {
                drive_joint_ang_rad[1] = msg->actual.positions[i];
                drive_joint_vel_rads[1] = msg->actual.velocities[i];
                //drive_joint_effort_NM[1] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "br_caster_r_wheel_joint")
            {
                drive_joint_ang_rad[2] = msg->actual.positions[i];
                drive_joint_vel_rads[2] = msg->actual.velocities[i];
                //drive_joint_effort_NM[2] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "fr_caster_r_wheel_joint")
            {
                drive_joint_ang_rad[3] = msg->actual.positions[i];
                drive_joint_vel_rads[3] = msg->actual.velocities[i];
                //drive_joint_effort_NM[3] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "fl_caster_rotation_joint")
            {
                steer_joint_ang_rad[0] = msg->actual.positions[i];
                steer_joint_vel_rads[0] = msg->actual.velocities[i];
                //steer_joint_effort_NM[0] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "bl_caster_rotation_joint")
            {
                steer_joint_ang_rad[1] = msg->actual.positions[i];
                steer_joint_vel_rads[1] = msg->actual.velocities[i];
                //steer_joint_effort_NM[1] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "br_caster_rotation_joint")
            {
                steer_joint_ang_rad[2] = msg->actual.positions[i];
                steer_joint_vel_rads[2] = msg->actual.velocities[i];
                //steer_joint_effort_NM[2] = msg->effort[i];
            }
            if(msg->joint_names[i] ==  "fr_caster_rotation_joint")
            {
                steer_joint_ang_rad[3] = msg->actual.positions[i];
                steer_joint_vel_rads[3] = msg->actual.velocities[i];
                //steer_joint_effort_NM[3] = msg->effort[i];
            }
        }

        // Set measured Wheel Velocities and Angles to Controler Class (implements inverse kinematic)
        ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads,
                                         drive_joint_ang_rad, steer_joint_ang_rad);


        // calculate odometry every time
        UpdateOdometry();

    }