<commit_before>// Copyright 2016 AUV-IITK
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <actionlib/server/simple_action_server.h>
#include <motion_commons/ForwardAction.h>
#include <dynamic_reconfigure/server.h>
#include <motion_forward/pidConfig.h>
#include <string>
using std::string;

typedef actionlib::SimpleActionServer<motion_commons::ForwardAction> Server;  // defining the Client type
float presentForwardPosition = 0;
float previousForwardPosition = 0;
float finalForwardPosition, error, output;
bool initData = false;
std_msgs::Int32 pwm;  // pwm to be send to arduino

// new inner class, to encapsulate the interaction with actionclient
class innerActionClass
{
private:
  ros::NodeHandle nh_;
  Server forwardServer_;
  std::string action_name_;
  motion_commons::ForwardFeedback feedback_;
  motion_commons::ForwardResult result_;
  ros::Publisher PWM;
  float p, i, d;

public:
  // Constructor, called when new instance of class declared
  explicit innerActionClass(string name)
    :  // Defining the server, third argument is optional
    forwardServer_(nh_, name, boost::bind(&innerActionClass::analysisCB, this, _1), false)
    , action_name_(name)
  {
    // Add preempt callback
    forwardServer_.registerPreemptCallback(boost::bind(&innerActionClass::preemptCB, this));
    // Declaring publisher for PWM
    PWM = nh_.advertise<std_msgs::Int32>("/pwm/forward", 1000);
    // Starting new Action Server
    forwardServer_.start();
  }

  // default contructor
  ~innerActionClass(void)
  {
  }

  // callback for goal cancelled
  void preemptCB(void)
  {
    pwm.data = 0;
    PWM.publish(pwm);
    ROS_INFO("pwm send to arduino %d", pwm.data);
    // this command cancels the previous goal
    forwardServer_.setPreempted();
  }

  // called when new goal recieved; Start motion and finish it, if not interupted
  void analysisCB(const motion_commons::ForwardGoalConstPtr goal)
  {
    ROS_INFO("Inside analysisCB");

    int count = 0;
    int loopRate = 10;
    ros::Rate loop_rate(loopRate);

    // waiting till we recieve the first value from Camera else it's useless to any calculations
    while (!initData)
    {
      ROS_INFO("Waiting to get first input from Camera");
      loop_rate.sleep();
    }

    if (goal->Goal == 1)
      finalForwardPosition = 0;

    float derivative = 0, integral = 0, dt = 1.0 / loopRate;
    bool reached = false;
    pwm.data = 0;

    if (!forwardServer_.isActive())
      return;

    while (!forwardServer_.isPreemptRequested() && ros::ok() && count < goal->loop)
    {
      error = finalForwardPosition - presentForwardPosition;
      integral += (error * dt);
      derivative = (presentForwardPosition - previousForwardPosition) / dt;
      output = (p * error) + (i * integral) + (d * derivative);
      forwardOutputPWMMapping(output);

      if (pwm.data < 2 && pwm.data > -2)
      {
        reached = true;
        pwm.data = 0;
        PWM.publish(pwm);
        ROS_INFO("thrusters stopped");
        count++;
      }
      else
      {
        reached = false;
        count = 0;
      }

      if (forwardServer_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        forwardServer_.setPreempted();
        reached = false;
        break;
      }<commit_msg>changed sign convention for forward motion<commit_after>// Copyright 2016 AUV-IITK
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <actionlib/server/simple_action_server.h>
#include <motion_commons/ForwardAction.h>
#include <dynamic_reconfigure/server.h>
#include <motion_forward/pidConfig.h>
#include <string>
using std::string;

typedef actionlib::SimpleActionServer<motion_commons::ForwardAction> Server;  // defining the Client type
float presentForwardPosition = 0;
float previousForwardPosition = 0;
float finalForwardPosition, error, output;
bool initData = false;
std_msgs::Int32 pwm;  // pwm to be send to arduino

// new inner class, to encapsulate the interaction with actionclient
class innerActionClass
{
private:
  ros::NodeHandle nh_;
  Server forwardServer_;
  std::string action_name_;
  motion_commons::ForwardFeedback feedback_;
  motion_commons::ForwardResult result_;
  ros::Publisher PWM;
  float p, i, d;

public:
  // Constructor, called when new instance of class declared
  explicit innerActionClass(string name)
    :  // Defining the server, third argument is optional
    forwardServer_(nh_, name, boost::bind(&innerActionClass::analysisCB, this, _1), false)
    , action_name_(name)
  {
    // Add preempt callback
    forwardServer_.registerPreemptCallback(boost::bind(&innerActionClass::preemptCB, this));
    // Declaring publisher for PWM
    PWM = nh_.advertise<std_msgs::Int32>("/pwm/forward", 1000);
    // Starting new Action Server
    forwardServer_.start();
  }

  // default contructor
  ~innerActionClass(void)
  {
  }

  // callback for goal cancelled
  void preemptCB(void)
  {
    pwm.data = 0;
    PWM.publish(pwm);
    ROS_INFO("pwm send to arduino %d", pwm.data);
    // this command cancels the previous goal
    forwardServer_.setPreempted();
  }

  // called when new goal recieved; Start motion and finish it, if not interupted
  void analysisCB(const motion_commons::ForwardGoalConstPtr goal)
  {
    ROS_INFO("Inside analysisCB");

    int count = 0;
    int loopRate = 10;
    ros::Rate loop_rate(loopRate);

    // waiting till we recieve the first value from Camera else it's useless to any calculations
    while (!initData)
    {
      ROS_INFO("Waiting to get first input from Camera");
      loop_rate.sleep();
    }

    if (goal->Goal == 1)
      finalForwardPosition = 0;

    float derivative = 0, integral = 0, dt = 1.0 / loopRate;
    bool reached = false;
    pwm.data = 0;

    if (!forwardServer_.isActive())
      return;

    while (!forwardServer_.isPreemptRequested() && ros::ok() && count < goal->loop)
    {
      error = presentForwardPosition - finalForwardPosition;
      integral += (error * dt);
      derivative = (presentForwardPosition - previousForwardPosition) / dt;
      output = (p * error) + (i * integral) + (d * derivative);
      forwardOutputPWMMapping(output);

      if (pwm.data < 2 && pwm.data > -2)
      {
        reached = true;
        pwm.data = 0;
        PWM.publish(pwm);
        ROS_INFO("thrusters stopped");
        count++;
      }
      else
      {
        reached = false;
        count = 0;
      }

      if (forwardServer_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        forwardServer_.setPreempted();
        reached = false;
        break;
      }
{'fraction_non_alphanumeric': 0.11203501094091904, 'fraction_numerical': 0.01050328227571116, 'mean_word_length': 3.2320987654320987, 'pattern_counts': {'":': 0, '<': 29, '<?xml version=': 0, '>': 31, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6196849', 'n_tokens_mistral': 2200, 'n_tokens_neox': 2074, 'n_words': 561}