How to create AckermannDriveStamped messages in python

ackermann_msg

asked 2017-07-06 19:43:10 -0500

Ryan_F
71 ●7 ●8 ●15

updated 2018-07-22 00:39:21 -0500

jayess
6155 ●26 ●85 ●92

I'm very, very new to ROS and i'm working on my own MIT RACECAR (for those who are unfamiliar http://fast.scripts.mit.edu/racecar/ ). I have a ROS network of nodes and topics to control an ackermann type vehicle platform. There are built in topics that pass along AckermannDriveStamped messages to lower level nodes that do some math/filtering which actually make my car run, and i'm interested in creating my own series of ackermann message and publishing them just to see the car maybe run through a short series of directions autonomously.

I'm wondering how to actually write python code in AckermannDriveStamped format. Here's what I have so far:

import rospy

import yaml

from sensor_msgs.msg import Joy #i'm also trying to play around with Joy messages which is why this import is included

import numpy as np

import std_msgs.msg

import ackermann_msgs.AckermannDriveStamped #this isn't correct, i'm not sure what to import here

header = rospy.Header()

axes = {"axes:[-0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, 0.0]"}

buttons = {"buttons":"[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]"}

command = rospy.Header(),axes,buttons

       print(header) 
       print("-------------")
       print(buttons) 
       print("-------------")
       print(command) 
       print("----------------------------")
           rospy.loginfo(command)
           pub.publish(command)
           rate.sleep()

When i runs that ROS spits back :

TypeError: Invalid number of arguments, args should be ['header', 'axes', 'buttons'] args are((seq: 0
stamp: 
  secs: 0
  nsecs:         0
frame_id: '', set(['axes:[-0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, 0.0]']), {'buttons': '[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]'}),)

It's obviously littered with syntax errors and i don't expect to be able to pass an actual array called "buttons", but i did expect there to be some sort of class like rospy.Header than i can use which ROS will understand.

Can anyone provide me some source code for actually creating and publishing AckermannDriveStamped messages or help me out?

PS I'm really sorry about the poor formatting of the code in this question. I couldn't get it to work properly

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2017-07-07 01:46:41 -0500

gstavrinos

641 ●2 ●10 ●16 http://gstavrinos.github.io/

import rospy
from sensor_msgs.msg import Joy
from ackermann_msgs.msg import AckermannDriveStamped

ack_publisher = None
max_speed = 1 # m/s
max_steering = 1.047198 # radians

def joyCallback(msg):
    for i in range(len(msg.buttons)):
        if msg.buttons[i] == 1:
            print 'WOW! You pressed the ' + str(i) + ' button!'

    ack_msg = AckermannDriveStamped()
    ack_msg.header.stamp = rospy.Time.now()
    ack_msg.header.frame_id = 'your_frame_here'
    ack_msg.drive.steering_angle = msg.axes[0] * max_steering
    ack_msg.drive.speed = msg.axes[1] * max_speed
    ack_publisher.publish(ack_msg)

if __name__ == '__main__':
    rospy.init_node('Ryan_F_example')
    max_speed = rospy.get_param("~max_speed", 1)
    max_steering = rospy.get_param("~max_steering", 1.047198)
    rospy.Subscriber('joy', Joy, joyCallback)
    ack_publisher = rospy.Publisher('your/topic/here', AckermannDriveStamped, queue_size=1)
    while not rospy.is_shutdown():
        rospy.spin()

I have not tested the code, but it should work with some minor corrections. I don't see why you would want to publish joy messages, so in this example I have subscribed to the joy topic, and based on that I am publishing AckermannDriveStamped messages.

A small explanation of the code:

Starting from main, we first initialize our node, get two parameters for max_speed and max_steering, which are based on our robot and can be acquired from a yaml file that is fed to our code using a launch file. We then subscribe to the joy topic, and create a publisher for AckermannDriveStamped messages. The following while ensures that the node will trigger its callback(s) for as long as it is alive.

Continuing with the joyCallback method, we first check if there are any buttons pressed and print a dummy message there. You can remove this code, or use it based on your needs and your robot's capabilities (e.g. X button used as horn and Y button as headlights!). After taking care of the buttons, we come to the interesting part: The axes! In this example I assume that you use the left analog stick to control the robot. axes[0] is left/right in a range of [-1,1] (with left being the positive) and axes[1] is forward/back in a range of [-1,1] (with forward being the positive). Regarding your Header question, you should NEVER touch the seq field of the header, because it is automatically generated. It is also a good practice to fill in the frame_id field.

I think I have covered the basics. Hit me up if you have any more problems! Good luck!

edit flag offensive delete link

Comments

Thanks much! This was exactly what i needed! If i have any other questions i will let you know. Thanks a bunch for your help

Ryan_F ( 2017-07-10 12:44:18 -0500 )edit

you should NEVER touch the seq field of the header, because it is automatically generated.

in reality: seq is actually deprecated, and not used (consistently) any more. So not using it is good advice, but the why is only partially true.

gvdhoorn ( 2017-07-10 13:49:36 -0500 )edit

Can you please point me somewhere that I can see the seq deprecation?

gstavrinos ( 2017-07-11 01:22:02 -0500 )edit

See my answer to #q261184.

gvdhoorn ( 2017-07-11 04:00:39 -0500 )edit

Thank you!

gstavrinos ( 2017-07-11 04:04:41 -0500 )edit

add a comment

answered 2018-07-01 19:38:12 -0500

AutoCar
102 ●50 ●57 ●59

updated 2019-07-29 22:26:27 -0500

jayess
6155 ●26 ●85 ●92

Here is how to do this in C++:

#ifndef __RUN_MAP_H__
#define __RUN_MAP_H__


#include <math.h>
#include <stdlib.h>
#include <string>
#include <vector>

#include <ros/ros.h>

#include <time.h>
#include <sensor_msgs/Joy.h>
#include <ackermann_msgs/AckermannDriveStamped.h>

#define RUNMAP_DEFAULT_MAX_SPEED     0.5
#define RUNMAP_DEFAULT_MAX_STEERING  1.0477198
#define RUNMAP_DEFAULT_SPEED_SCALE   0.1

class RunMap
{
public:
    RunMap(ros::NodeHandle* nodehandle);

private:

    ros::NodeHandle    _nh;
    ros::Subscriber    _subscriber;
    ros::Publisher     _publisher;

    float _MAX_SPEED;
    float _MAX_STEERING;
    float _SPEED_SCALE;


private:
    void initializeSubscribers();
    void initializePublishers();

    void subscriberCallback(const sensor_msgs::Joy& message_holder);
};

#endif


#include "run_map.h"
   RunMap::RunMap(ros::NodeHandle* nodehandle)
        :_nh(*nodehandle),
         _MAX_SPEED( RUNMAP_DEFAULT_MAX_SPEED),
         _MAX_STEERING(RUNMAP_DEFAULT_MAX_STEERING),
         _SPEED_SCALE(RUNMAP_DEFAULT_SPEED_SCALE)
    {
        ROS_INFO("Creating RunMap");
        initializeSubscribers();
        initializePublishers();
    }

    void RunMap::initializeSubscribers()
    {
        ROS_INFO("Initializing Subscribers");
        _subscriber = _nh.subscribe("/joy",
                                    1,
                                    &RunMap::subscriberCallback,
                                    this);
    }

    void RunMap::initializePublishers()
    {
        ROS_INFO("Initializing Publishers");
        _publisher = _nh.advertise<ackermann_msgs::AckermannDriveStamped>("myproject/runmap_pub", 1, true);
    }

    void RunMap::subscriberCallback(const sensor_msgs::Joy& message_holder) {
            if (message_holder.buttons[1]){ //button 1 pressed

               //do something here to find the speed and steering. 

                float speed = xxxx;
                if (speed > _MAX_SPEED){
                    speed = _MAX_SPEED;
                }
                ROS_INFO("speed:%f", speed);

                float steering = yyyy;
                if (steering > _MAX_STEERING){
                    steering = _MAX_STEERING;
                }
                ROS_INFO("steering:%f", steering);

                ackermann_msgs::AckermannDriveStamped output_msg;
                output_msg.header.stamp = ros::Time::now();
                output_msg.header.frame_id = "";
                output_msg.drive.steering_angle = steering;
                output_msg.drive.speed = speed;

                _publisher.publish(output_msg);
            }
        }


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

    ros::NodeHandle nh;

    ROS_INFO("Instantiating an object of type RunMap");
    RunMap runMap(&nh);

    ROS_INFO("Going into spin; callbacks is waiting...");
    ros::spin();

    return 0;
}

When you create the package, make it dependent on roscpp. Then build it under the workspace with catkin_make.