Creating a teleop node/cmd_vel publisher [closed]
I'm trying to piece all of this together and have a couple questions. I am building a robot "Inga" who will run a pair of pittman motors through an Arduino Mega ADK with an Adafruit motor controller shield and a Robogaia encoder shield, ROS Hydro on 64 bit Toshiba Satellite running Ubuntu Precise. I'm getting comfortable with ROS but still at the shallow end of the learning curve. I know just enough C++ to not entirely helpless, am pretty good with python. I'm choosing to write most of my stuff in C++ to help me in my learning there. My ultimate goal is to employ the Nav stack using a Kinect and an XV-11 Lidar, plus some infrareds and sonars. In the process of learning how to do all of this I'm starting with just being able to move the bot around using a ps2 joystick and then build up to the Nav stack from there as my knowledge and time allows. I've read the appropriate tutorials for joy including creating a teleop node as well as all the Nav stack tutorials and ROS by Example.
Here's what I think I know: 1) I need to write code for the Arduino that subscribes to gemoetry/Twist msgs to get cmd_vel and that publishes the encoder data. This is almost done. 2) I need to create a base controller that subscribes to the joystick and publishes the cmd_vel messages the Arduino will read. This is what I'm working on now.
In order to create (2) I've copied the "Creating a Simple teleop node" code from [here] ( http://wiki.ros.org/joy/Tutorials/Wri... ), renamed it inga_joy.cpp and modified it as follows:
#include <ros/ros.h>
#include <turtlesim/Velocity.h>
#include <sensor_msgs/Joy.h>
class TeleopInga
{
public:
TeleopInga();
private:
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
ros::NodeHandle nh_;
int linear_, angular_;
double l_scale_, a_scale_;
ros::Publisher vel_pub_;
ros::Subscriber joy_sub_;
};
TeleopInga::TeleopInga():
linear_(1),
angular_(2)
{
nh_.param("axis_linear", linear_, linear_);
nh_.param("axis_angular", angular_, angular_);
nh_.param("scale_angular", a_scale_, a_scale_);
nh_.param("scale_linear", l_scale_, l_scale_);
vel_pub_ = nh_.advertise<inga::Velocity>("inga/command_velocity", 1);
joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopInga::joyCallback, this);
}
void TeleopInga::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
turtlesim::Velocity vel;
vel.angular = a_scale_*joy->axes[angular_];
vel.linear = l_scale_*joy->axes[linear_];
vel_pub_.publish(vel);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "teleop_inga");
TeleopInga teleop_inga;
ros::spin();
}
Essentially what I've done so far is create a pkg in catkin_ws/src named inga_teleop with dependencies joy roscpp rospy and std_msgs
THE QUESTIONS: 1. On line 2: #include <turtlesim velocity.h=""> - since I'm not using turtlesim, where do I get the header to include for Velocity.h?
On line 44: turtlesim::Velocity vel; - this will just be the package that I include for Velocity.h ?
I see that I'm using sensor msgs, should I have included them in my initial package, or are ...