Getting Stuck with Simple Teleop Twist Control

asked 2021-08-22 00:30:48 -0600

bobjks1 gravatar image

Hello,

I'm new to ROS and still in the learning stages. I have a simple 4 wheel robot built and can write Arduino code to control the motors (forward, backward, left, right). Obviously, I now want to try and control these motions in real time. I'm trying to wrap my head around teleop_twist_keyboard and what is actually happening so I can write code that sends control inputs to the Arduino.

I run the following commands:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

rosrun rosserial_python serial_node.py /dev/ttyACM0

I think this sets up the publisher/subscriber which seems to work. My issue is getting the actual Arduino to respond to teleop_twist_keyboard. I found the code below and edited it hoping it would allow my keyboard inputs to do simple forward/backward movements but nothing happens.

For reference, my 6 (left) and 10 (right) pins correspond to forward for each side of the robot. The 5 (left) and 11 (right) pins correspond to reverse on each side.

include <ros.h>

#include <geometry_msgs/Twist.h>
float x;
float y; 
ros::NodeHandle nh;

void velCallback(const geometry_msgs::Twist& vel)
{
   if (x = vel.linear.x - 1.0) //Forward move.
      {
      analogWrite(6, 255);
      analogWrite(10, 255);
      }
else if (y = vel.linear.y - 1.0)   //Backward move
{
      analogWrite(5, 255);
      analogWrite(11, 255);
}
else
{
    analogWrite(5, 0);     //STOP
    analogWrite(6, 0);
    analogWrite(10, 0);
    analogWrite(11, 0);
}
    }

ros::Subscriber<geometry_msgs::twist> sub("cmd_vel" , velCallback);

void setup() { nh.initNode(); nh.subscribe(sub); }

void loop() { nh.spinOnce(); delay(10); }

Can someone walk me through what is actually happening with the rosserial node and how it interacts with the Arduino code? What is actually being received on the /cmd_vel topic and how do I convert this to PWM inputs to the Arduino?

edit retag flag offensive close merge delete