Problem interfacing laptop keyboard and arduino via ROS

asked 2019-06-13 02:25:31 -0500

dw007 gravatar image

updated 2019-06-13 11:10:28 -0500

jayess gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

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

jayess gravatar image jayess  ( 2019-06-13 03:04:06 -0500 )edit

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

dw007 gravatar image dw007  ( 2019-06-13 03:59:02 -0500 )edit

enter code #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(); }

dw007 gravatar image dw007  ( 2019-06-13 04:08:36 -0500 )edit

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

dw007 gravatar image dw007  ( 2019-06-13 04:09:47 -0500 )edit

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

jayess gravatar image jayess  ( 2019-06-13 11:11:24 -0500 )edit