Commit fb58c3c7 authored by Christian Sponfeldner's avatar Christian Sponfeldner

WIP: New auxilliary drive node

parent 08ace8b2
cmake_minimum_required(VERSION 2.8.3)
project(auxiliary_drive)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
emros_dispatcher
message_generation
utility_ofc
emros_common
)
################################################
## 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 exec_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 exec_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
# )
## 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
# )
###################################
## 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 your 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 auxiliary_drive
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ executable
add_executable(${PROJECT_NAME}_node src/auxiliary_drive_node.cpp src/auxiliary_drive.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
# #############
# ## Install ##
# #############
#
#
# ## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME}_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(DIRECTORY launch
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#
\ No newline at end of file
#ifndef AUXILIARY_DRIVE_HPP
#define AUXILIARY_DRIVE_HPP
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "emros_dispatcher/eth.h"
#include "utility_ofc/ethprotocol.h"
#include "emros_common/emros_commands.h"
#include "emros_common/attributes.h"
#include "emros_common/kinematics_kind_configuration.h"
class AuxiliaryDrive
{
public:
AuxiliaryDrive();
~AuxiliaryDrive();
private:
void TargetPositionCallback(std_msgs::Float64::ConstPtr msg);
void DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr& msg);
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* getTargetPositionsInstance();
EMROSCmdType::AuxiliaryController::ActualPositionsInterface* getActualPositionsInstance();
std::string getKindOfKinematicsName();
ros::NodeHandle mNodeHandler;
ros::Publisher mPub_dispatcher_tx;
ros::Subscriber mSub_dispatcher_rx;
ros::Subscriber mSub_target_position;
const Robot::KindOfKinematics mKindOfKinematics;
const emros_dispatcher::eth::_id_type mID_emros_device = EMROSCmdType::AuxiliaryController::DEVICE_ID;
};
#endif // AUXILIARY_DRIVE_HPP
<?xml version="1.0"?>
<package format="2">
<name>auxiliary_drive</name>
<version>0.0.1</version>
<description>The auxiliary_drive package</description>
<maintainer email="christian.sponfeldner@nanotec.de">Christian Sponfeldner</maintainer>
<license>MIT</license>
<url type="website">https://gitlab.nanotec.de/nathan/ros-packages</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<depend>emros_dispatcher</depend>
<build_depend>utility_ofc</build_depend>
<build_depend>emros_common</build_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<export>
</export>
</package>
#include "auxiliary_drive/auxiliary_drive.hpp"
AuxiliaryDrive::AuxiliaryDrive():
mKindOfKinematics(Robot::GetKindOfKinematics())
{
mPub_dispatcher_tx = mNodeHandler.advertise<emros_dispatcher::eth>("dispatcher_tx", 5);
mSub_dispatcher_rx = mNodeHandler.subscribe("dispatcher_rx", 1000, &AuxiliaryDrive::DispatcherRxCallback, this);
mSub_target_position = mNodeHandler.subscribe("target_position", 1, &AuxiliaryDrive::TargetPositionCallback, this);
}
AuxiliaryDrive::~AuxiliaryDrive()
{
}
void AuxiliaryDrive::TargetPositionCallback(std_msgs::Float64::ConstPtr msg)
{
namespace AuxNS = EMROSCmdType::AuxiliaryController;
emros_dispatcher::eth tx_msg;
AuxNS::TargetPositionsInterface &target_position_in_radian { *getTargetPositionsInstance()};
tx_msg.cmd = AuxNS::TARGET_POSITION;
tx_msg.id = mID_emros_device;
tx_msg.payload.resize(target_position_in_radian.getRawSize());
//TODO: error handling
AuxNS::TargetPositions2WDplusAuxiliary &pos = dynamic_cast<AuxNS::TargetPositions2WDplusAuxiliary&> (target_position_in_radian);
//TODO NULLPTR ?
pos.data().auxiliary = int(msg->data);
target_position_in_radian.get(&tx_msg.payload[0], tx_msg.payload.size());
mPub_dispatcher_tx.publish(tx_msg);
}
void AuxiliaryDrive::DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr& msg)
{
}
inline EMROSCmdType::AuxiliaryController::TargetPositionsInterface* AuxiliaryDrive::getTargetPositionsInstance()
{
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* pReturn;
switch (mKindOfKinematics)
{
case Robot::KindOfKinematics::TWO_WD:
pReturn = new EMROSCmdType::AuxiliaryController::TargetPositions2WD;
break;
case Robot::KindOfKinematics::TWO_WD_PLUS_AUX:
pReturn = new EMROSCmdType::AuxiliaryController::TargetPositions2WDplusAuxiliary;
break;
case Robot::KindOfKinematics::FOUR_WD:
pReturn = new EMROSCmdType::AuxiliaryController::TargetPositions4WD;
break;
default:
ROS_ERROR("Kind of kinematics is not supported: %s", getKindOfKinematicsName().c_str() );
break;
}
return (pReturn);
}
inline std::string AuxiliaryDrive::getKindOfKinematicsName()
{
std::string kindOfKinematicsName = "Unknown kind of kinematics";
switch (mKindOfKinematics)
{
case Robot::KindOfKinematics::TWO_WD:
kindOfKinematicsName = "TWO_WD";
break;
case Robot::KindOfKinematics::TWO_WD_PLUS_AUX:
kindOfKinematicsName = "TWO_WD_PLUS_AUX";
break;
case Robot::KindOfKinematics::FOUR_WD:
kindOfKinematicsName = "FOUR_WD";
break;
case Robot::KindOfKinematics::UNDEFINED:
kindOfKinematicsName = "UNDEFINED";
break;
}
return (kindOfKinematicsName);
}
//inline ActualVelocitiesInterface* AuxiliaryController::getActualVelocitiesInstance()
//{
// ActualVelocitiesInterface* pReturn;
// switch (mBaseConfig.mode_of_operation)
// {
// case KinematicMode::DIFFERENTIAL:
// pReturn = new ActualWheelVelocities2WD;
// break;
// case KinematicMode::DIFFERENTIAL_PLUS_AUXILIARY:
// pReturn = new ActualWheelVelocities2WDplusAuxiliary;
// break;
// case KinematicMode::MECANUM:
// pReturn = new ActualWheelVelocities4WD;
// break;
// }
// return (pReturn);
//}
#include "auxiliary_drive/auxiliary_drive.hpp"
#include "emros_common/emros_commands.h"
#include "ros/ros.h"
#include <iostream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "auxiliary_drive");
AuxiliaryDrive auxiliary_drive;
ros::spin();
return 0;
}
Subproject commit 66d9097972bcf004bec6d6868b7f816103edd9f2
Subproject commit d8b325c263e750d710fb35fde959c91ce83da895
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment