Robotics StackExchange | Archived questions

Problem interfacing laptop keyboard and arduino via ROS

So I am working on the movement of my robot using arrow keys. I have used the teleop_turtle code and changed it according to my application. It publishes flag whenever it detects that key has been pressed but when no key is pressed there is no data being published. I have a variable in Arduino which is storing the flag message but when no message is being published it uses the old flag message as a reference and continues with that operation. It will work if I directly connect the motor to my UP board but I have to first figure the way to solve this. Can anyone please help me with this? Thanks!

Publisher node:

#include <ros/ros.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include <std_msgs/Int32.h>
#define KEYCODE_R 0x43 
#define KEYCODE_L 0x44
#define KEYCODE_U 0x41
#define KEYCODE_D 0x42
#define KEYCODE_Q 0x71
#define KEYCODE_S 0x73


std_msgs::Int32 flag_msg;
std_msgs::Int32 DAC_msg;

class TeleopTurtle
{
public:
   TeleopTurtle();
   void keyLoop();

private:

    ros::NodeHandle nh_;
    double linear_, angular_, l_scale_, a_scale_;
    ros::Publisher movement_pub = nh_.advertise<std_msgs::Int32>("/movement", 1000);
    ros::Publisher DAC_pub = nh_.advertise<std_msgs::Int32>("/DAC_data", 1000);
};

TeleopTurtle::TeleopTurtle():
   linear_(0),
   angular_(0),
   l_scale_(2.0),
   a_scale_(2.0)
 {
   nh_.param("scale_angular", a_scale_, a_scale_);
   nh_.param("scale_linear", l_scale_, l_scale_);
}

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
    tcsetattr(kfd, TCSANOW, &cooked);
    ros::shutdown();
    exit(0);
}


int main(int argc, char** argv)
{
   ros::init(argc, argv, "manual_mode");


  TeleopTurtle teleop_turtle;

   signal(SIGINT,quit);

   teleop_turtle.keyLoop();

  return(0);
}

void TeleopTurtle::keyLoop()
{
   char c;
   bool dirty=false;


// get the console in raw mode                                                              

   tcgetattr(kfd, &cooked);
   memcpy(&raw, &cooked, sizeof(struct termios));
   raw.c_lflag &=~ (ICANON | ECHO);
   // Setting a new line, then end of file                         
   raw.c_cc[VEOL] = 1;
   raw.c_cc[VEOF] = 2;
   tcsetattr(kfd, TCSANOW, &raw);

   puts("Reading from keyboard");
   puts("---------------------------");
   puts("Use arrow keys to move the truck.");

 for(;;)

while(1)
{
     // get the next event from the keyboard  
    if(read(kfd, &c, 1) < 0)
    {
       perror("read():");
       exit(-1);
    }

    ROS_DEBUG("value: 0x%02X\n", c);


    if(c==KEYCODE_L)
    {
        ROS_DEBUG("LEFT");
        //publish to make left stop and right move
        flag_msg.data=3;
        dirty = true;
    }
    else if(c==KEYCODE_R)
    {
        ROS_DEBUG("RIGHT");
       //publish to make right stop and left move
       flag_msg.data=2;
       dirty = true;
    }
    else if(c==KEYCODE_U)
    {
        ROS_DEBUG("UP");
       //publish to make both move ahead
       flag_msg.data=1;
       dirty = true;
       DAC_msg.data=1;
    }

    else if(c==KEYCODE_D)
    {
        ROS_DEBUG("DOWN");
        //publish to make both move backward
        flag_msg.data=-1;
        dirty = true;
        DAC_msg.data=1;

    }
    else if(c==KEYCODE_S)      
    {
        ROS_DEBUG("Stop");
        //publish to stop
        flag_msg.data=0;
        DAC_msg.data=0;
        dirty = true;
    }


    if(dirty ==true)
    {
       movement_pub.publish(flag_msg);    
       DAC_pub.publish(DAC_msg);
       dirty=false;
     }

  }

  return;
}

Arduino

#include <ros.h>
#include <std_msgs/Int32.h>

ros::NodeHandle  nh;
int test_led=12;
int fwd;
int bwk;
bool dirty=false;

std_msgs::Int32 flag_msg;
int flag;
void messageCb( const std_msgs::Int32& msg){
//  digitalWrite(LED_BUILTIN, msg.data);   // blink the led
    flag=msg.data;
}

ros::Subscriber<std_msgs::Int32> sub("movement", &messageCb );

void setup()
{ 
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(test_led, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{  
  switch(flag)
     {
       case 1:
       fwd=1;
       bwk=1;
       dirty = true;
       break;
       case -1:
       fwd=0;
       bwk=0;
       dirty = true;
       break;
       case 2:
       fwd=1;
       bwk=0;
       dirty = true;
       break;
       case 3:
       fwd=0;
       bwk=1;
       dirty = true;
       break;
       }
 if(dirty==true)
     {
      digitalWrite(LED_BUILTIN, fwd);
      digitalWrite(test_led, bwk);
      dirty=false;
     }
  nh.spinOnce();
}

Asked by dw007 on 2019-06-13 02:25:31 UTC

Comments

Can anyone please help me with this?

That's going to be difficult with only a description of your code versus seeing your actual code

Asked by jayess on 2019-06-13 03:04:06 UTC

@jayess I have added the publisher node. I will add the arduino code too

Asked by dw007 on 2019-06-13 03:59:02 UTC

enter code #include

include

ros::NodeHandle nh; int test_led=12; int fwd; int bwk; bool dirty=false;

std_msgs::Int32 flag_msg; int flag; void messageCb( const std_msgs::Int32& msg){ // digitalWrite(LED_BUILTIN, msg.data); // blink the led flag=msg.data; }

ros::Subscriber sub("movement", &messageCb );

void setup() { pinMode(LED_BUILTIN, OUTPUT); pinMode(test_led, OUTPUT); nh.initNode(); nh.subscribe(sub); }

void loop() {
switch(flag) { case 1: fwd=1; bwk=1; dirty = true; break; case -1: fwd=0; bwk=0; dirty = true; break; case 2: fwd=1; bwk=0; dirty = true; break; case 3: fwd=0; bwk=1; dirty = true; break; } if(dirty==true) { digitalWrite(LED_BUILTIN, fwd); digitalWrite(test_led, bwk); dirty=false; } nh.spinOnce(); }

Asked by dw007 on 2019-06-13 04:08:36 UTC

@jayess I have used 2 leds for testing rn then I will change.

Asked by dw007 on 2019-06-13 04:09:47 UTC

@dw007 Please don't use comments to post code, that's not what they're for. In the future, please update your question instead

Asked by jayess on 2019-06-13 11:11:24 UTC

Answers