Move it.cpp v1

/*
 *  move_it
 *  Turn toward a tracked object.
 *
 *  Copyright (C) 2012, Ed C. Epp
 *  http://zdome.net/wiki/index.php/ROS
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <ros/ros.h>
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "cv_bridge/cv_bridge.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Twist.h"
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <math.h>

#define SCREEN_WIDTH 320
#define MID_X (SCREEN_WIDTH / 2)
#define MIN_POWER 1
#define MAX_POWER 5
#define TURN_POWER 3

ros::Publisher mover;

// ---------- trackCallback --------------------------------------------------
void trackCallback (const geometry_msgs::Point::ConstPtr & track_msg)
  {
    ros::NodeHandle nh;
    geometry_msgs::Twist move_msg;

    // Set the turn - turn on the z axis
    move_msg.linear.x = 0;
    move_msg.linear.y = 0;
    move_msg.linear.z = 0;
    move_msg.angular.x = 0;
    move_msg.angular.y = 0;

    // Do the P part of PID
    //move_msg.angular.z = 
    //        (track_msg->x - MID_X) / MID_X *                 // magnitude
    //        (TURN_POWER - MIN_POWER) +                       // scale
    //        (MID_X < track_msg->x ? MIN_POWER : -MIN_POWER); // offset
    move_msg.angular.z = 
      (track_msg->x - MID_X) / MID_X * MAX_POWER;

    // Execute turn
    mover.publish(move_msg);

    ROS_INFO("%f %d %d\n", (track_msg->x - MID_X) / MID_X, 
	                   (TURN_POWER - MIN_POWER),
	                   (MID_X < track_msg->x ? MIN_POWER : -MIN_POWER));

    // Debugging Output
    ROS_INFO("trackCallback x: %d  y: %d  radius: %d    result: %f",
	     (int)track_msg->x, (int)track_msg->y, (int)track_msg->z,
             move_msg.angular.z);
  }


// -----------------------------------------------------------------------------
// ---------- main -------------------------------------------------------------
// -----------------------------------------------------------------------------
int
main (int argc, char **argv)
{
  // Initialize ROS Node
  ros::init (argc, argv, "moveNode");
  ros::NodeHandle nh;

  mover = nh.advertise<geometry_msgs::Twist>
              ("/cmd_vel", 1000);
  ros::Subscriber sub = nh.subscribe("/tracker", 1000, 
                    trackCallback);

  // Run forever
  ros::spin ();

  return 0;
}