Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
add vesc_ackermann package for translating between vesc and ackermann…
… and odometry messages
  • Loading branch information
Michael Boulet committed Jan 15, 2016
1 parent c426d32 commit c4ffaa4
Show file tree
Hide file tree
Showing 21 changed files with 715 additions and 43 deletions.
74 changes: 74 additions & 0 deletions vesc_ackermann/CMakeLists.txt
@@ -0,0 +1,74 @@
cmake_minimum_required(VERSION 2.8.3)
project(vesc_ackermann)

find_package(catkin REQUIRED COMPONENTS
nodelet
pluginlib
roscpp
nav_msgs
std_msgs
ackermann_msgs
vesc_msgs
)

catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS nodelet pluginlib roscpp nav_msgs std_msgs ackermann_msgs vesc_msgs
)

###########
## Build ##
###########

include_directories(
include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)

# node executable
add_executable(vesc_to_odom_node src/vesc_to_odom_node.cpp
src/vesc_to_odom.cpp)
target_link_libraries(vesc_to_odom_node
${catkin_LIBRARIES}
)

add_executable(ackermann_to_vesc_node src/ackermann_to_vesc_node.cpp
src/ackermann_to_vesc.cpp)
target_link_libraries(ackermann_to_vesc_node
${catkin_LIBRARIES}
)

# nodelet library
add_library(vesc_ackermann_nodelet src/ackermann_to_vesc_nodelet.cpp
src/ackermann_to_vesc.cpp
src/vesc_to_odom_nodelet.cpp
src/vesc_to_odom.cpp)
target_link_libraries(vesc_ackermann_nodelet
${catkin_LIBRARIES}
)

#############
## Install ##
#############

install(TARGETS vesc_to_odom_node ackermann_to_vesc_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

install(TARGETS vesc_ackermann_nodelet
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
install(FILES vesc_ackermann_nodelet.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)

#############
## Testing ##
#############

# TODO
37 changes: 37 additions & 0 deletions vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h
@@ -0,0 +1,37 @@
// -*- mode:c++; fill-column: 100; -*-

#ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_
#define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_

#include <ros/ros.h>
#include <ackermann_msgs/AckermannDriveStamped.h>

namespace vesc_ackermann
{

class AckermannToVesc
{
public:

AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh);

private:
// ROS parameters
// conversion gain and offset
double speed_to_erpm_gain_, speed_to_erpm_offset_;
double steering_to_servo_gain_, steering_to_servo_offset_;

/** @todo consider also providing an interpolated look-up table conversion */

// ROS services
ros::Publisher erpm_pub_;
ros::Publisher servo_pub_;
ros::Subscriber ackermann_sub_;

// ROS callbacks
void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd);
};

} // namespace vesc_ackermann

#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_
47 changes: 47 additions & 0 deletions vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
@@ -0,0 +1,47 @@
// -*- mode:c++; fill-column: 100; -*-

#ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_
#define VESC_ACKERMANN_VESC_TO_ODOM_H_

#include <ros/ros.h>
#include <vesc_msgs/VescStateStamped.h>
#include <std_msgs/Float64.h>

namespace vesc_ackermann
{

class VescToOdom
{
public:

VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh);

private:
// ROS parameters
std::string odom_frame_;
std::string base_frame_;
/** State message does not report servo position, so use the command instead */
bool use_servo_cmd_;
// conversion gain and offset
double speed_to_erpm_gain_, speed_to_erpm_offset_;
double steering_to_servo_gain_, steering_to_servo_offset_;
double wheelbase_;

// odometry state
double x_, y_, yaw_;
std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value
vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message

// ROS services
ros::Publisher odom_pub_;
ros::Subscriber vesc_state_sub_;
ros::Subscriber servo_sub_;

// ROS callbacks
void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state);
void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo);
};

} // namespace vesc_ackermann

#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_
14 changes: 14 additions & 0 deletions vesc_ackermann/launch/ackermann_to_vesc_node.launch
@@ -0,0 +1,14 @@
<!-- -*- mode: XML -*- -->
<launch>

<!-- Optionally launch in GDB, for debugging -->
<arg name="debug" default="false" />
<arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />

<!-- Ackermann to VESC node -->
<node pkg="vesc_ackermann" type="ackermann_to_vesc_node" name="ackermann_to_vesc_node"
output="screen" launch-prefix="$(arg launch_prefix)" >
</node>

</launch>
29 changes: 29 additions & 0 deletions vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch
@@ -0,0 +1,29 @@
<!-- -*- mode: XML -*- -->
<launch>

<arg name="start_manager" default="true" />
<arg name="manager" default="vesc_nodelet_manager" />

<!-- Optionally launch manager in GDB, for debugging -->
<arg name="debug" default="false" />
<arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<!-- Worker threads -->
<arg name="num_worker_threads" default="4" />

<!-- Also globally disable bond heartbeat timeout in debug mode, so everything
doesn't die when you hit a break point -->
<param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />

<!-- Nodelet manager -->
<node if="$(arg start_manager)" pkg="nodelet" type="nodelet" name="$(arg manager)"
args="manager" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="num_worker_threads" value="$(arg num_worker_threads)" />
</node>

<!-- Ackermann to VESC nodelet -->
<node pkg="nodelet" type="nodelet" name="ackermann_to_vesc_nodelet"
args="load vesc_ackermann::AckermannToVescNodelet $(arg manager)" >
</node>

</launch>
14 changes: 14 additions & 0 deletions vesc_ackermann/launch/vesc_to_odom_node.launch
@@ -0,0 +1,14 @@
<!-- -*- mode: XML -*- -->
<launch>

<!-- Optionally launch in GDB, for debugging -->
<arg name="debug" default="false" />
<arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />

<!-- VESC to Odom node -->
<node pkg="vesc_ackermann" type="vesc_to_odom_node" name="vesc_to_odom_node"
output="screen" launch-prefix="$(arg launch_prefix)" >
</node>

</launch>
29 changes: 29 additions & 0 deletions vesc_ackermann/launch/vesc_to_odom_nodelet.launch
@@ -0,0 +1,29 @@
<!-- -*- mode: XML -*- -->
<launch>

<arg name="start_manager" default="true" />
<arg name="manager" default="vesc_nodelet_manager" />

<!-- Optionally launch manager in GDB, for debugging -->
<arg name="debug" default="false" />
<arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<!-- Worker threads -->
<arg name="num_worker_threads" default="4" />

<!-- Also globally disable bond heartbeat timeout in debug mode, so everything
doesn't die when you hit a break point -->
<param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />

<!-- Nodelet manager -->
<node if="$(arg start_manager)" pkg="nodelet" type="nodelet" name="$(arg manager)"
args="manager" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="num_worker_threads" value="$(arg num_worker_threads)" />
</node>

<!-- VESC to Odom nodelet -->
<node pkg="nodelet" type="nodelet" name="vesc_to_odom_nodelet"
args="load vesc_ackermann::VescToOdomNodelet $(arg manager)" >
</node>

</launch>
39 changes: 39 additions & 0 deletions vesc_ackermann/package.xml
@@ -0,0 +1,39 @@
<?xml version="1.0"?>
<package>
<name>vesc_ackermann</name>
<version>0.0.1</version>
<description>
Translate between VESC messages and ROS ackermann and odometry messages.
</description>

<maintainer email="boulet@ll.mit.edu">Michael T. Boulet</maintainer>
<author email="boulet@ll.mit.edu">Michael T. Boulet</author>
<license>BSD</license>

<url type="website">http://www.ros.org/wiki/vesc_ackermann</url>
<url type="repository">https://github.mit.edu/racecar/racecar-iap</url>
<url type="bugtracker">https://github.mit.edu/racecar/racecar-iap/issues</url>

<buildtool_depend>catkin</buildtool_depend>

<build_depend>nodelet</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>ackermann_msgs</build_depend>
<build_depend>vesc_msgs</build_depend>

<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>ackermann_msgs</run_depend>
<run_depend>vesc_msgs</run_depend>

<export>
<nodelet plugin="${prefix}/vesc_ackermann_nodelet.xml"/>
</export>

</package>
64 changes: 64 additions & 0 deletions vesc_ackermann/src/ackermann_to_vesc.cpp
@@ -0,0 +1,64 @@
// -*- mode:c++; fill-column: 100; -*-

#include "vesc_ackermann/ackermann_to_vesc.h"

#include <cmath>
#include <sstream>

#include <std_msgs/Float64.h>

namespace vesc_ackermann
{

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value);

AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh)
{
// get conversion parameters
if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_))
return;
if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_))
return;
if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_))
return;
if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_))
return;

// create publishers to vesc electric-RPM (speed) and servo commands
erpm_pub_ = nh.advertise<std_msgs::Float64>("commands/motor/speed", 10);
servo_pub_ = nh.advertise<std_msgs::Float64>("commands/servo/position", 10);

// subscribe to ackermann topic
ackermann_sub_ = nh.subscribe("ackermann_cmd", 10, &AckermannToVesc::ackermannCmdCallback, this);
}

typedef ackermann_msgs::AckermannDriveStamped::ConstPtr AckermannMsgPtr;
void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd)
{
// calc vesc electric RPM (speed)
std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64);
erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_;

// calc steering angle (servo)
std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64);
servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_;

// publish
if (ros::ok()) {
erpm_pub_.publish(erpm_msg);
servo_pub_.publish(servo_msg);
}
}

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value)
{
if (nh.getParam(name, value))
return true;

ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str());
return false;
}

} // namespace vesc_ackermann
16 changes: 16 additions & 0 deletions vesc_ackermann/src/ackermann_to_vesc_node.cpp
@@ -0,0 +1,16 @@
#include <ros/ros.h>

#include "vesc_ackermann/ackermann_to_vesc.h"

int main(int argc, char** argv)
{
ros::init(argc, argv, "ackermann_to_vesc_node");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");

vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh);

ros::spin();

return 0;
}

0 comments on commit c4ffaa4

Please sign in to comment.