Commit a3bfeeb7 authored by Sydney Speckle's avatar Sydney Speckle

initial commit

parent 386be628
# ---> C++
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
*.pyc
cmake_minimum_required(VERSION 2.8.3)
project(emros_basecontroller)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
emros_dispatcher
geometry_msgs
tf
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
#eth.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
# Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# geometry_msgs
# dispatcher
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES basecontroller
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
CATKIN_DEPENDS message_runtime
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
../emros_common_code/inc
${catkin_INCLUDE_DIRS}
)
#add_executable(mecanum_basecontroller src/mecanum_basecontroller.cpp )
#add_dependencies(mecanum_basecontroller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
#target_link_libraries(mecanum_basecontroller ${catkin_LIBRARIES} )
#add_executable(test_mecanum_basecontroller src/test_mecanum_basecontroller.cpp )
#add_dependencies(test_mecanum_basecontroller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
#target_link_libraries(test_mecanum_basecontroller ${catkin_LIBRARIES} )
#add_executable(diff_basecontroller src/diff_basecontroller.cpp )
#add_dependencies(diff_basecontroller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
#target_link_libraries(diff_basecontroller ${catkin_LIBRARIES} )
add_executable(basecontroller src/basecontroller_node.cpp src/basecontroller.cpp src/kinematics.cpp)
add_dependencies(basecontroller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
target_link_libraries(basecontroller ${catkin_LIBRARIES} )
## Declare a C++ library
# add_library(sepp_basecontroller
# src/${PROJECT_NAME}/sepp_basecontroller.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(sepp_basecontroller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
# add_executable(sepp_basecontroller_node src/sepp_basecontroller_node.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(sepp_basecontroller_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(sepp_basecontroller_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS sepp_basecontroller sepp_basecontroller_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_sepp_basecontroller.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
/*
* baseconroller.hpp
*
* Created on: Feb 9, 2017
* Author: speckle_s
*/
#pragma once
#include "ros/ros.h"
#include "emros_dispatcher/eth.h"
#include "geometry_msgs/Twist.h"
#include "tf/transform_broadcaster.h"
#include "nav_msgs/Odometry.h"
#include "ethprotocol.h"
#include "emros_commands.h"
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <leaky_bucket_timeout.h>
#include "kinematics.hpp"
#include "velocities.h"
#include <cmath>
using namespace EMROSCmdType::Basecontroller;
class Basecontroller
{
public:
struct Position
{
double x;
double y;
double phiz;
};
Basecontroller();
~Basecontroller();
private:
void calcOdomTF(geometry_msgs::TransformStamped& odomTf, nav_msgs::Odometry odom);
void calcOdom(nav_msgs::Odometry& odom, geometry_msgs::Twist twist, ros::Duration dt);
void DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr& msg);
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);
void parsePayload(ActualVelocitiesInterface& vel, emros_dispatcher::eth msg);
void mainLoop();
void LeakyBucketControllLoop(const ros::TimerEvent& event);
void getParam();
TargetVelocitiesInterface* getTargetVelocitiesInstance();
ActualVelocitiesInterface* getActualVelocitiesInstance();
ros::NodeHandle mNodeHandler;
boost::thread* mMainLoopThread;
tf::TransformBroadcaster mTransformBroadcaster;
LeakyBucketTimeout<ros::Time, ros::Duration, boost::mutex::scoped_lock, ros::Time (*)()> mLeakyBucketTimeout;
ros::Subscriber mSub_sepp_dispatcher_rx;
ros::Subscriber mSub_twist_cmd;
ros::Publisher mPub_sepp_dispatcher_tx;
ros::Publisher mPub_odom;
ros::Publisher mPub_twist_rx;
ros::Timer mLeakyBucketTimer;
Kinematics *mKinematic;
Position mPosition;
Config mBaseConfig;
ros::Time mLastTime;
geometry_msgs::Twist mTarget_vel;
geometry_msgs::Twist mLast_vel;
boost::mutex mMutex;
ActualVelocitiesInterface* mActual_vel;
const int mID;
};
/*
* kinematics.hpp
*
* Created on: Feb 9, 2017
* Author: speckle_s
*/
#ifndef EMROS_BASECONTROLLER_INCLUDE_EMROS_BASECONTROLLER_KINEMATICS_HPP_
#define EMROS_BASECONTROLLER_INCLUDE_EMROS_BASECONTROLLER_KINEMATICS_HPP_
#include "ros/ros.h"
#include "emros_dispatcher/eth.h"
#include "geometry_msgs/Twist.h"
#include "tf/transform_broadcaster.h"
#include "nav_msgs/Odometry.h"
#include "emros_commands.h"
#include "velocities.h"
using namespace EMROSCmdType::Basecontroller;
//////////////////////INTERFACE/////////////////////////////////////////////
class Kinematics
{
public:
Kinematics()
{
}
virtual ~Kinematics()
{
}
virtual void fwdKin(geometry_msgs::Twist& twist, ActualVelocitiesInterface& vel)=0;
virtual void invKin(TargetVelocitiesInterface& wheel_vel_Umin, geometry_msgs::Twist& twist)=0;
};
///////////////////CONCRETE CLASSES///////////////////////////////////////////////
class MecanumKinematics : public Kinematics
{
public:
MecanumKinematics(Config config) :
mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
{
}
~MecanumKinematics()
{
}
void fwdKin(geometry_msgs::Twist& twist, ActualVelocitiesInterface& vel) override;
void invKin(TargetVelocitiesInterface& wheel_vel_Umin, geometry_msgs::Twist& twist) override;
private:
Config mConfig;
const double mWheel_radius;
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class DifferentialKinematics: public Kinematics
{
public:
DifferentialKinematics(Config config) :
mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
{
}
~DifferentialKinematics()
{
}
void fwdKin(geometry_msgs::Twist& twist, ActualVelocitiesInterface& vel) override;
void invKin(TargetVelocitiesInterface& wheel_vel_Umin, geometry_msgs::Twist& twist) override;
private:
Config mConfig;
const double mWheel_radius;
};
#endif /* EMROS_BASECONTROLLER_INCLUDE_EMROS_BASECONTROLLER_KINEMATICS_HPP_ */
<launch>
<arg name="cmdvel_map" default= "cmd_vel"/>
<param name="robot_description" command="$(find xacro)/xacro '$(find emros_general)/urdf/emmi01.xacro'" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node pkg="emros_basecontroller" type="basecontroller" name="basecontroller" output="screen">
<param name="mode_of_operation" value="DIFFERENTIAL"/>
<param name="width_robot" value="0.0.434"/>
<param name="wheel_diameter" value="0.10"/>
<param name="controller_frequency" value="20.0"/>
<remap from="cmd_vel" to="$(arg cmdvel_map)" />
</node>
</launch>
<launch>
<arg name="cmdvel_map" />
<!-- <rosparam file="$(find emros_teleop)/sepp_teleop.yaml" command="load" ns="teleop" /> -->
<param name="robot_description" command="$(find xacro)/xacro '$(find emros_general)/urdf/sepp.xacro'" />
<!-- <include file="$(find emros_dispatcher)/launch/dispatcher.launch" >
<arg name="target_ip" value="10.1.61.222" />
<arg name="source_ip" value="10.1.61.5" />
<arg name="port" value="65000" />
</include> -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node pkg="emros_basecontroller" type="basecontroller" name="basecontroller" output="screen">
<param name="mode_of_operation" value="MECANUM"/>
<param name="width_robot" value="0.380"/>
<param name="length_robot" value="0.4954"/>
<param name="wheel_diameter" value="0.10"/>
<param name="controller_frequency" value="20.0"/>
<remap from="cmd_vel" to="$(arg cmdvel_map)" />
</node>
</launch>
<?xml version="1.0"?>
<package>
<name>emros_basecontroller</name>
<version>0.0.0</version>
<description>The sepp_basecontroller package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="speckle.speckle@nanotec.de">speckle sydney</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/sepp_basecontroller</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>
<build_depend>emros_dispatcher</build_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>emros_dispatcher</run_depend>
<run_depend>geometry_msgs</run_depend>
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags=""/>
</export>
</package>
\ No newline at end of file
/*
* basecontroller.cpp
*
* Created on: Feb 10, 2017
* Author: speckle_s
*/
#include "emros_basecontroller/basecontroller.hpp"
Basecontroller::Basecontroller() :
mPosition(), mTarget_vel(), mLeakyBucketTimeout(ros::Duration(0.3), 48, 6, 3, ros::Time::now), mID(
DEVICE_ID)
{
getParam();
switch (mBaseConfig.mode_of_operation)
{
case ModeOfOperation::DIFFERENTIAL:
mKinematic = new DifferentialKinematics(mBaseConfig);
break;
case ModeOfOperation::MECANUM:
mKinematic = new MecanumKinematics(mBaseConfig);
break;
}
mActual_vel = getActualVelocitiesInstance();
mPub_sepp_dispatcher_tx = mNodeHandler.advertise<emros_dispatcher::eth>("dispatcher_tx", 5);
mPub_twist_rx = mNodeHandler.advertise<geometry_msgs::Twist>("cmd_rx", 10);
mPub_odom = mNodeHandler.advertise<nav_msgs::Odometry>("odom", 10);
mSub_sepp_dispatcher_rx = mNodeHandler.subscribe("dispatcher_rx", 1000, &Basecontroller::DispatcherRxCallback, this);
mSub_twist_cmd = mNodeHandler.subscribe("cmd_vel", 1, &Basecontroller::cmdVelCallback, this);
mLeakyBucketTimer = mNodeHandler.createTimer(ros::Duration(0.01), &Basecontroller::LeakyBucketControllLoop, this);
mMainLoopThread = new boost::thread(boost::bind(&Basecontroller::mainLoop, this));
}
Basecontroller::~Basecontroller()
{
mMainLoopThread->join();
delete mMainLoopThread;
delete mKinematic;
delete mActual_vel;
}
inline TargetVelocitiesInterface* Basecontroller::getTargetVelocitiesInstance()
{
TargetVelocitiesInterface* pReturn;
switch (mBaseConfig.mode_of_operation)
{
case ModeOfOperation::DIFFERENTIAL:
pReturn = new TargetWheelVelocities2WD;
break;
case ModeOfOperation::MECANUM:
pReturn = new TargetWheelVelocities4WD;
break;
}
return (pReturn);
}
inline ActualVelocitiesInterface* Basecontroller::getActualVelocitiesInstance()
{
ActualVelocitiesInterface* pReturn;
switch (mBaseConfig.mode_of_operation)
{
case ModeOfOperation::DIFFERENTIAL:
pReturn = new ActualWheelVelocities2WD;
break;
case ModeOfOperation::MECANUM:
pReturn = new ActualWheelVelocities4WD;
break;
}
return (pReturn);
}
void Basecontroller::getParam()
{
std::string mode_of_operation;
ros::param::param<std::string>("~/mode_of_operation", mode_of_operation, "DIFFERENTIAL");
ros::param::param<double>("~/width_robot", mBaseConfig.width_robot, 0.380);
ros::param::param<double>("~/length_robot", mBaseConfig.length_robot, 0.4954);
ros::param::param<double>("~/wheel_diameter", mBaseConfig.wheel_dia, 0.10);
ros::param::param<double>("~/controller_frequency", mBaseConfig.controller_frequency, 20.0);
ROS_INFO("Basecontroller started with %s",mode_of_operation.c_str());
if (mode_of_operation.compare("DIFFERENTIAL") == 0)
{
mBaseConfig.mode_of_operation = ModeOfOperation::DIFFERENTIAL;
}
else if (mode_of_operation.compare("MECANUM") == 0)
{
mBaseConfig.mode_of_operation = ModeOfOperation::MECANUM;
}
else
{
//TODO: ERROR (exception)
}
}
void Basecontroller::LeakyBucketControllLoop(const ros::TimerEvent& event)
{
mLeakyBucketTimeout.update();
}
void Basecontroller::mainLoop()
{
ros::Rate rate(mBaseConfig.controller_frequency);
TargetVelocitiesInterface &wheel_vel_Umin { *getTargetVelocitiesInstance()};
while (ros::ok())
{
wheel_vel_Umin.clearData();
geometry_msgs::Twist vel_cmd;
emros_dispatcher::eth tx_msg;
{
boost::mutex::scoped_lock lock(mMutex);
if (mLeakyBucketTimeout.checkError())
{
mTarget_vel.angular.x = 0;
mTarget_vel.angular.y = 0;
mTarget_vel.angular.z = 0;
mTarget_vel.linear.x = 0;
mTarget_vel.linear.y = 0;
mTarget_vel.linear.z = 0;
}
vel_cmd = mTarget_vel;
}
if(mTarget_vel.angular.z != mLast_vel.angular.z || mTarget_vel.linear.x != mLast_vel.linear.x || mTarget_vel.linear.y != mLast_vel.linear.y)
{
mKinematic->invKin(wheel_vel_Umin, vel_cmd);
tx_msg.cmd = TARGET_VELOCITIES;
tx_msg.id = mID;
tx_msg.payload.resize(wheel_vel_Umin.getRawSize());
//TODO: error handling
wheel_vel_Umin.get(&tx_msg.payload[0], tx_msg.payload.size());
mPub_sepp_dispatcher_tx.publish(tx_msg);
mLast_vel = mTarget_vel;
}
rate.sleep();
}
delete &wheel_vel_Umin;
}
void Basecontroller::DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr& msg)
{
if (msg->id == mID)
{
ros::Duration dt;
mActual_vel->clearData();
geometry_msgs::Twist twist;
nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odomTf;