can i keep teleop ,getting encoder data and publishing tf data in single arduino code ?

asked 2019-09-22 01:58:23 -0500

kallivalli gravatar image

updated 2019-09-30 07:01:02 -0500

hello everyone,

I have arduino mega in which i wrote a program for controlling motor ,teleop and publishing tf and subscribing odom data , while doing slam gmapping my tf_frame shows map > odom (broadcaster is serial_node) and I don't get any output in rostopic echo /Odom is this because I haven't rosrun Odom through node ??

I have differential drive and I'm trying to do gmapping ?

here is code

#include <ros.h>
#include <ros/time.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
int dir1leftWheel = 22;
int dir2leftWheel = 24;
int vel_leftWheel = 10; 

int dir1rightWheel = 26;
int dir2rightWheel = 28;
int vel_rightWheel = 9; 

int right_pwm, left_pwm,left_pwmf,right_pwmf;
float slope=120/1;

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;
void msg(const geometry_msgs::Twist& cmd){
  float vel_right = (cmd.angular.z*51)/2 + cmd.linear.x;
  float vel_left = cmd.linear.x*2-vel_right;
  left_pwm=vel_left*slope;
  right_pwm=vel_right*slope;
  analogWrite(vel_leftWheel, left_pwm);
    digitalWrite(dir1leftWheel, HIGH);
    digitalWrite(dir2leftWheel, LOW);
    analogWrite(vel_rightWheel, right_pwm);
    digitalWrite(dir1rightWheel, HIGH);
    digitalWrite(dir2rightWheel, LOW);
}

double x = 0.0;                         // Initial X position
double y = 0.0;                         // Initial Y position
double theta = 0.00;                    // Initial Theta angle

char base_link[] = "/base_link";
char odom[] = "/odom";
ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", &msg);

volatile signed int counterL = 0;       // This variable will increase or decrease depending on the rotation of encoder
volatile signed int counterR = 0;       // This variable will increase or decrease depending on the rotation of encoder
volatile int dcountL = 0;               // diff in encoder reading for left wheel
volatile int dcountR = 0;               // diff in encoder reading for left wheel
float pi=3.14;                          // Pi=3.14
float R=0.115;                          // Wheel Radius 
unsigned int tick=400;                  // Encoder total tick
float len=0.60;                         // Distance between two wheels


void setup()
{
  nh.initNode();                        // Initializing node handler
  broadcaster.init(nh);                 // odom data broadcaster init
    nh.subscribe(sub);
  pinMode(2, INPUT);
  pinMode(4, INPUT);
  digitalWrite(2, HIGH);                // Left encoder input
  digitalWrite(4, HIGH);
  attachInterrupt(0, ai0, RISING); 

  pinMode(3, INPUT);
  pinMode(5, INPUT);                    // Right encoder input
  digitalWrite(3, HIGH); 
  digitalWrite(5, HIGH); 
  attachInterrupt(1, ai1, RISING);
  pinMode(dir1leftWheel, OUTPUT);
  pinMode(dir2leftWheel, OUTPUT);
  pinMode(vel_leftWheel, OUTPUT);
  pinMode(dir1rightWheel, OUTPUT);
  pinMode(dir2rightWheel, OUTPUT);
  pinMode(vel_rightWheel, OUTPUT);

}

void loop()
{ 
  dcountL=counterL-dcountL;
  dcountR=counterR-dcountR;

  signed int templ=counterL; 
  signed int tempR=counterR;

  float dL=0.72*(dcountL/(float)tick);  // Dl = 2*PI*R*(lefttick/totaltick)
  float dR=0.72*(dcountR/(float)tick);  // Dr = 2*PI*R*(righttick/totaltick)
  float dC=(dL+dR)/2;

   x = x+(dC*(cos(theta)));             // calculates new X position based on wheel revolution
   y = y+(dC*(sin(theta)));             // calculates new Y position based on wheel revolution
   theta = theta +((dR-dL)/len);        // calculates new theta angle based on encoder values
  if(theta > 3.14)
    theta=-3.14;

  // tf odom->base_link
  t.header.frame_id = odom;             // odom data publishes on Odom topic
  t.child_frame_id = base_link;         

  t.transform.translation.x = x;
  t.transform.translation.y = y;
  // converting from euler angle to quaternion form
  t.transform.rotation = tf::createQuaternionFromYaw(theta);
  t.header.stamp = nh.now ...
(more)
edit retag flag offensive close merge delete

Comments

I would suggest that you give a bit more context here. Can you post your code so others can get a better sense of what you are doing?

kscottz gravatar imagekscottz ( 2019-09-27 12:51:33 -0500 )edit

@kscottz i have updated can you please look on it ?

kallivalli gravatar imagekallivalli ( 2019-09-29 23:41:21 -0500 )edit

I don't see anything that jumps out at me right away. Just so we are clear the problem is that you can subscribe to a topic but not publish a topic? If this were me I would start really simply and just see if the publish works, then test if the subscribed topics works. This might help you to better isolate the error. If it were me I would also fiddle with the delays/spins/interrupts and see if something is not re-entering where you think it should be.

kscottz gravatar imagekscottz ( 2019-09-30 13:14:57 -0500 )edit

its motor control code which works fine and teleop too ,but only issue is publishing odom data which i think i need to have seperate odometry publisher , as im also in confusion how to take two encoders data send it to odometry and transform for gmapping

kallivalli gravatar imagekallivalli ( 2019-09-30 23:05:47 -0500 )edit

If taking input to the Arduino -> doing the transform -> publishing the transform -> outputting to motors then will there be a delay in input and output?

kallivalli gravatar imagekallivalli ( 2019-10-01 01:27:16 -0500 )edit