TF vibrating error on the rviz when arduino-ros communication

asked 2017-11-12 06:25:31 -0600

Mekateng gravatar image

I am trying to publish the joint_state data in arduino.When I run the Rviz, there is vibrating in TF. The problems of the youtube link and my code are below.I also get a warning in Arduino;Low memory available,stability problems may occur.Can this problem arise from the arduino? https://www.youtube.com/watch?v=QHuvs...

#include <Wire.h>
#include "Arduino.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/JointState.h>
#include <ros/time.h>




#define MD22ADDRESS         0x58                              // address of md 22 (all mode switches on)
#define SOFTREG             0x07                              // Byte for reading software register
#define MOTOR1              0x01                              // Byte for first motor
#define MOTOR2              0x02                              // Byte for second motor
#define ACCELLREG           0x03                              // Byte to set acelleration
#define AEncoderInterrupt 0 
#define BEncoderInterrupt 1
#define EncoderPinA 2   // enkoder 2 ve 3 numaralı pine bağlanacak
#define EncoderPinB 3
volatile bool EncoderBSet;
volatile bool EncoderASet;

int pozisyon = 0;
float ang=0;
float encoder_resolution=4200;

//const char *a = "base_tilt_joint";
char *a[] = {"base_tilt_joint"}; // F: Front - B: Back - R: Right - L: Left

float 

pos[]={0.0}; /// stores arduino time
float vel[]={0.0};
float eff[]={0.0};

ros::NodeHandle nh;
std_msgs::Header header;

sensor_msgs::JointState robot_state;
ros::Publisher chatter("joint_states", &robot_state);

void setup() {
 // Serial.begin(115200);
   Wire.begin();


  // delay(100);
  nh.getHardware()->setBaud(230400);
  nh.initNode();
  nh.advertise(chatter);
  delay(10);




 robot_state.header;
         robot_state.header.stamp=nh.now();
  robot_state.name_length = 1;
  robot_state.velocity_length = 1;
  robot_state.position_length = 1; /// here used for arduino time
  robot_state.effort_length = 1; /// here used for arduino time

robot_state.name = a;
pos[0]=degreTOrad(pozisyon);
    robot_state.position = pos;
robot_state.velocity = vel;

robot_state.effort = eff;




// delay(100);
  pinMode(EncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(EncoderPinA, HIGH);  // turn on pullup resistors
  pinMode(EncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(EncoderPinB, HIGH);  // turn on pullup resistors
  attachInterrupt(AEncoderInterrupt, HandleMotorInterruptA, CHANGE);
  attachInterrupt(BEncoderInterrupt, HandleMotorInterruptB, CHANGE);
 setMode();  
}

void loop(){    
 // Serial.println(pozisyon);
  //delay(10);{

 //pozisyon=0;

  //robot_state.header.stamp=nh.now();

   ang=((pozisyon/encoder_resolution)*360.0);  // calculate angel according to pulse count


     ang=int(ang)%360;


        pos[0]=degreTOrad(ang);
    robot_state.header.stamp=nh.now();
 //   robot_state.name = a;
    robot_state.position = pos;
  //  robot_state.velocity = vel;
 //   robot_state.effort = eff;
   chatter.publish( &robot_state );
   pozisyon++;



  if(pozisyon==8400){pozisyon=0;}
  // delay(10);



  Wire.beginTransmission(MD22ADDRESS);                // Set first motor to a speed of 255
  Wire.write(MOTOR1);
  Wire.write(220);
  Wire.endTransmission();
nh.spinOnce();
  //delay(1);
}

void setMode(){
  Wire.beginTransmission(MD22ADDRESS);                // Set a value of 255 to the acceleration register
  Wire.write(ACCELLREG);
  Wire.write(0xFF);
  Wire.endTransmission();
}


 void HandleMotorInterruptA()
{

  if (digitalRead(EncoderPinB) == digitalRead(EncoderPinA))
  {
    if(pozisyon == 0) pozisyon = 8400;
        else pozisyon--;
  }
      else
      { 
            if(pozisyon == 8400) pozisyon = 0;
        else po

zisyon++;
  }
}

void HandleMotorInterruptB()


 {
      if (digitalRead(EncoderPinB) == digitalRead(EncoderPinA))
      {
            if(pozisyon == 8400) pozisyon = 0;
        else pozisyon++;
      }



     else
      {
            if

(pozisyon == 0) pozisyon = 8400;
        else pozisyon--;
  }
}



  float degreTOrad(float degre){



  return (degre/57.2958);

  }
edit retag flag offensive close merge delete