ROS Arduino Serial Issues [closed]

edit

Hello! I am attempting to send encoder values over serial from an arduino device to the master device using the Vector3 stamped message. Currently, the encoders work using test code but once I add ros communication the Encoder values are not being sent.

See below for my current arduino code.

I am using the rosserial serial_node.py on the master to transmit published data from the Arduino.

I am running Ubuntu 18.04 on x86 with ros melodic.

Code:

#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <PID_v1.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#include <Encoder.h>

Encoder myEnc(4, 5);
Encoder myEnc2(2, 3);

float x, z;

int enA = 5, in1 = 3, in2 = 4, enB = 6, in3 = 2, in4 = 7, enC = 10, in5 = 12, in6 = 11, enD = 9, in7 = 13, in8 = 8;

long oldPosition = -999, oldPosition2 = -999;

#define LOOPTIME 100     //Looptime in millisecond
const byte noCommLoopMax = 10;                //number of main loops the robot will execute without communication before stopping
unsigned int noCommLoops = 0;                 //main loop without communication counter

unsigned long lastMilli = 0;

const double radius = 0.1016;                   //.   Wheel radius, in m
const double wheelbase = 0.4;               //.   Wheelbase, in m
const double encoder_cpr = 28.65;               //.   Encoder ticks or counts per rotation 
const double speed_to_pwm_ratio = 0.00235;    //Ratio to convert speed (in m/s) to PWM value. It was obtained by plotting the wheel speed in relation to the PWM motor command (the value is the slope of the linear function).
const double min_speed_cmd = 0.0882;          //(min_speed_cmd/speed_to_pwm_ratio) is the minimum command value needed for the motor to start moving. This value was obtained by plotting the wheel speed in relation to the PWM motor command (the value is the constant of the linear function).

double speed_req = 0;                         //Desired linear speed for the robot, in m/s
double angular_speed_req = 0;                 //Desired angular speed for the robot, in rad/s

double speed_req_left = 0;                    //Desired speed for left wheel in m/s
double speed_act_left = 0;                    //Actual speed for left wheel in m/s
double speed_cmd_left = 0;                    //Command speed for left wheel in m/s 

double speed_req_right = 0, speed_act_right = 0, speed_cmd_right = 0; //Command speed for wheels in m/s 

const double max_speed = 0.4;                 //Max speed in m/s

int PWM_leftMotor = 0, PWM_rightMotor = 0;  //PWM commands

// PID Parameters
const double PID_left_param[] = { 0, 0, 0.1 }; //Respectively Kp, Ki and Kd for left motor PID
const double PID_right_param[] = { 0, 0, 0.1 }; //Respectively Kp, Ki and Kd for right motor PID

double pos_left = 0, pos_right = 0;      //encoder positions

PID PID_leftMotor(&speed_act_left, &speed_cmd_left, &speed_req_left, PID_left_param[0], PID_left_param[1], PID_left_param[2], DIRECT);          //Setting up the PID for left motor
PID PID_rightMotor(&speed_act_right, &speed_cmd_right, &speed_req_right, PID_right_param[0], PID_right_param[1], PID_right_param[2], DIRECT);   //Setting up the PID for right motor

ros::NodeHandle nh;

void velcallback(const geometry_msgs::Twist& cmd_vel) {

  speed_req = cmd_vel.linear.x;                                     //Extract the commanded linear speed from the message
  angular_speed_req = cmd_vel.angular.z;                            //Extract the commanded angular speed from the message

  speed_req_left = speed_req - angular_speed_req*(wheelbase/2 ...