assignment 2

assignment_2/ajit2704_pid_control/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+set (SRCS1 ${SRCS1} src/Point.cpp) 
+set (SRCS1 ${SRCS1} src/node.cpp) 
+set (SRCS1 ${SRCS1} src/pid.cpp)
+include_directories(include ${catkin_INCLUDE_DIRS})
+add_executable(pid ${SRCS1}) 
+target_link_libraries(pid ${catkin_LIBRARIES}) 

assignment_2/ajit2704_pid_control/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

assignment_2/ajit2704_pid_control/include/header.h

@@ -0,0 +1,23 @@
+#include <math.h> 
+#include "ros/ros.h"
+
+class Point
+{
+public:
+
+
+  Point(double xpos, double ypos, double theta, ros::Time t);
+
+  ~Point();
+
+
+  double getAngle(Point* target);
+
+
+  double getDistance(Point* target);
+
+  double x;      
+  double y;      
+  ros::Time time;
+  double angle;  
+};

assignment_2/ajit2704_pid_control/include/node.h

@@ -0,0 +1,45 @@
+#include "header.h"
+#include "ros/ros.h"
+#include "nav_msgs/Odometry.h"
+#include "geometry_msgs/Twist.h"
+#include <queue>
+
+class NodePID
+{
+public:
+
+
+  NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed);
+
+  ~NodePID();
+
+
+  void publishMessage(double angleCommand, double speedCommand);
+
+ /* This method reads data from sensor and processes them to variables.
+   */
+
+  void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+  /*if robot is close enough from target point, target is accomplished and method returns true.
+   */
+  bool closeEnough(Point* actual);
+
+  /* This method calculates action intervention from PSD controller.
+   */
+  double calculatePSD(Point* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum);
+
+//variables
+  Point *start;     // Start position. Distance will be measured from here
+  Point *last;      // Last position of robot.
+  double tolerance;   // Tolerated deviation from target distance.
+  double maxSpeed;    // Maximal velocity.
+  double maxASpeed;   // Maximal angular velocity.
+  ros::Publisher pubMessage; // Object for publishing messages.
+  double targetDistance; // Robot will go forward this distance.
+  double targetAngle;    // Robot will turn by this angle.
+  int iterations;        // Number of received messages.
+  double sumDistance;    // Sum of distance errors for PSD controller.
+  double sumAngle;       // Sum of angle errors for PSD controller.
+  double toleranceAngle; // Tolerated deviation from target angle.
+};

assignment_2/ajit2704_pid_control/manifest.xml

@@ -0,0 +1,14 @@
+<package>
+  <description brief="ajit2704_pid_control">
+
+     ajit2704_pid_control
+
+  </description>
+  <author>ajit</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ajit2704_pid_control</url>
+
+</package>
+
+

assignment_2/ajit2704_pid_control/src/header.cpp

@@ -0,0 +1,23 @@
+#include "header.h"
+
+Point::Point(double xpos, double ypos, double theta, ros::Time t)
+{
+  x = xpos;
+  y = ypos;
+  angle = theta;
+  time = t;
+}
+
+Point::~Point()
+{
+}
+
+double Point::getAngle(Point* target)
+{
+  return atan2((target->y - y),(target->x - x));
+}
+
+double Point::getDistance(Point* target)
+{
+  return sqrt((pow(target->y - y,2.0)) + (pow(target->x - x,2.0)));
+}

assignment_2/ajit2704_pid_control/src/node.cpp

@@ -0,0 +1,123 @@
+#include "node.h"
+#include <math.h>
+#define PI 3.141592
+#define T_KP 2.58  // P constant for PSD translation controller
+#define T_KD 0.047  // D constant for PSD translation controller
+#define T_KI 0.0  // S constant for PSD translation controller
+#define R_KP 2.0  // P constant for PSD rotation controller
+#define R_KD 0.1  // D constant for PSD rotation controller
+#define R_KI 0.0  // S constant for PSD rotation controller
+
+NodePID::NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed)
+{
+  targetDistance = dist;
+  tolerance = tol;
+  toleranceAngle = tolA;
+  targetAngle = ang;
+  maxSpeed = mSpeed;
+  maxASpeed = mASpeed;  
+  pubMessage = pub;
+  iterations = 0;
+  sumDistance = 0;
+  sumAngle = 0;
+  start = new Point(0.0, 0.0, 0.0, ros::Time::now());
+  last = new Point(0.0, 0.0, 0.0, ros::Time::now());
+}
+
+NodePID::~NodePID()
+{
+}
+
+//Publisher
+void NodePID::publishMessage(double angleCommand, double speedCommand)
+{
+
+  geometry_msgs::Twist msg;
+
+  msg.linear.x = speedCommand;
+  msg.angular.z = angleCommand;
+
+  //publishing message
+  pubMessage.publish(msg);
+}
+
+//Subscriber
+void NodePID::messageCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+  double angleCommand = 0;
+  double speedCommand = 0;
+  Point* actual = new Point(msg->pose.pose.position.x, msg->pose.pose.position.y, 2.0*asin(msg->pose.pose.orientation.z), msg->header.stamp);
+  if (closeEnough(actual) == true)
+  {
+    ROS_INFO("Target found");
+    publishMessage(0.0,0.0);
+    exit(0);
+  }
+  if (iterations == 0)
+  {
+    start->x = actual->x;
+    start->y = actual->y;
+    start->time = actual->time;
+    start->angle = actual->angle;
+    last->x = actual->x;
+    last->y = actual->y;
+    last->time = actual->time;
+    last->angle = actual->angle;
+  }
+  iterations++;
+
+
+  if (fabs(targetDistance) > tolerance)
+  {
+    speedCommand = calculatePSD(actual,start->getDistance(actual)*copysign(1.0, targetDistance),start->getDistance(last)*copysign(1.0, targetDistance),targetDistance,T_KP,T_KD,T_KI,&sumDistance);
+  }
+
+  if (actual->angle-last->angle < -PI)
+  {
+    actual->angle += 2*PI;
+  } 
+  else if (actual->angle-last->angle > PI)
+  {
+    actual->angle -= 2*PI;
+  }
+
+  angleCommand = calculatePSD(actual,actual->angle-start->angle, last->angle-start->angle,targetAngle,R_KP,R_KD,R_KI,&sumAngle);
+
+  //Saving position to last
+  last->x = actual->x;
+  last->y = actual->y;
+  last->time = actual->time;
+  last->angle = actual->angle;
+
+
+  publishMessage(fmin(maxASpeed,angleCommand), fmin(maxSpeed,speedCommand));
+}
+
+bool NodePID::closeEnough(MyPoint* actual)
+{
+  double distance;
+  distance = start->getDistance(actual)*copysign(1.0, targetDistance);
+  if (fabs(distance-targetDistance) > tolerance)
+  {
+    return false;
+  }
+  if (fabs(targetAngle - (actual->angle - start->angle)) > toleranceAngle &
+    fabs(targetAngle - (actual->angle - start->angle) + 2*PI) > toleranceAngle &
+    fabs(targetAngle - (actual->angle - start->angle) - 2*PI) > toleranceAngle)
+  {
+    return false;
+  }
+  return true;
+}
+
+double NodePID::calculatePSD(MyPoint* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum)
+{
+  double speed = 0;
+  double error = reference - actualValue;
+  double previousError = reference - lastValue;
+  double dt = actual->time.toSec() - last->time.toSec();
+  double derivative = (error - previousError)/dt;
+  *sum = *sum + error*dt;
+  speed = kP*error + kD*derivative + kS*(*sum);
+  return speed;
+}

assignment_2/ajit2704_pid_control/src/pid.cpp

@@ -0,0 +1,34 @@
+#include "header.h"
+#include "node.h"
+
+#define SUBSCRIBER_SIZE 1  // Size of buffer for subscriber.
+#define PUBLISHER_SIZE 1000  // Size of buffer for publisher.
+#define TOLERANCE 0.01  //at which the distance will be considered as achieved.
+#define TOLERANCE_ANGLE 0.02  // Differenc from target angle, which will be tolerated.
+#define MAX_SPEED 0.5    // Maximum speed of robot.
+#define MAX_A_SPEED 2.0    // Maximum angular speed of robot.
+#define PUBLISHER_TOPIC "/cmd_vel"
+#define SUBSCRIBER_TOPIC "odom"
+#define PI 3.141592
+
+int main(int argc, char **argv)
+{
+  //Initialization of node
+  ros::init(argc, argv, "pid");
+  ros::NodeHandle n;
+
+
+
+  //Creating publisher
+  ros::Publisher pubMessage = n.advertise<geometry_msgs::Twist>(PUBLISHER_TOPIC, PUBLISHER_BUFFER_SIZE);
+
+  //Creating object, which stores data from sensors and has methods for
+  //publishing and subscribing
+  NodePID *nodePID = new NodePID(pubMessage, TOLERANCE, TOLERANCE_ANGLE, distance, angle, MAX_SPEED, MAX_A_SPEED);
+
+  //Creating subscriber 
+  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodePID::messageCallback, nodePID);
+  ros::spin();
+
+  return 0;
+}