Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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
Showing
21 changed files
with
715 additions
and
43 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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; | ||
} |
Oops, something went wrong.