Commit ded036a4 authored by Svetlana's avatar Svetlana

ROS cpp package with generated source

parent bb4bcffa
The goal of this project is a model driven design of deep learning based architectures for the TORCS platform.
\ No newline at end of file
## Install ROS
### Versions
Ubuntu 16.04
ROS lunar
Install libarmadillo-dev package
sudo ln -s /usr/include/armadillo /usr/include/armadillo.h
Follow instructions on http://wiki.ros.org/lunar/Installation/Ubuntu
Follow instructions to create package on http://wiki.ros.org/ROS/Tutorials/CreatingPackage
We use RosWorkspace/ as the ROS workspace directory and RosWorkspace/src/torcs2 as the package directory example.
Generate sources via EMAMRosCpp
Copy sources into created package src directory (RosWorkspace/src/torcs2/src)
Find string:
# add_executable(${PROJECT_NAME}_node src/torcs2_node.cpp)
in the RosWorkspace/src/torcs2/CMakeLists.txt file, uncomment and change to:
add_executable(${PROJECT_NAME} src/torcs2.cpp)
Find and uncomment in the same file:
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
and remove _node suffix to have:
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
)
Run catkin_make and catkin_make install as described in tutorial.
Check that package is now available:
rospack list | grep torcs
Run node with the package:
rosrun torcs torcs
Have fun!
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 2.8.3)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path})
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()
/opt/ros/lunar/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
cmake_minimum_required(VERSION 2.8.3)
project(torcs)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-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
std_msgs
)
## 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
# )
## 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
# )
################################################
## 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 torcs
# CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/torcs.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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME} src/torcs.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
## same as for the library above
# 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}
${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 ${PROJECT_NAME} ${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(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_torcs.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)
<?xml version="1.0"?>
<package>
<name>torcs</name>
<version>0.0.0</version>
<description>The torcs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="alex@todo.todo">alex</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 multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/torcs</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, 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>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
#ifndef HELPERA_H
#define HELPERA_H
#define _GLIBCXX_USE_CXX11_ABI 0
#include <iostream>
#include "armadillo.h"
#include <stdarg.h>
#include <initializer_list>
using namespace arma;
class HelperA{
public:
static mat getEigenVectors(mat A){
vec eigenValues;
mat eigenVectors;
eig_sym(eigenValues,eigenVectors,A);
return eigenVectors;
}
static vec getEigenValues(mat A){
vec eigenValues;
mat eigenVectors;
eig_sym(eigenValues,eigenVectors,A);
return eigenValues;
}
static mat getKMeansClusters(mat A, int k){
mat clusters;
kmeans(clusters,A.t(),k,random_subset,20,true);
printf("cluster centroid calculation done\n");
std::ofstream myfile;
myfile.open("data after cluster.txt");
myfile << A;
myfile.close();
std::ofstream myfile2;
myfile2.open("cluster centroids.txt");
myfile2 << clusters;
myfile2.close();
mat indexedData=getKMeansClustersIndexData(A.t(), clusters);
std::ofstream myfile3;
myfile3.open("data after index.txt");
myfile3 << indexedData;
myfile3.close();
return indexedData;
}
static mat getKMeansClustersIndexData(mat A, mat centroids){
mat result=mat(A.n_cols, 1);
for(int i=0;i<A.n_cols;++i){
result(i, 0) = getIndexForClusterCentroids(A, i, centroids);
}
return result;
}
static int getIndexForClusterCentroids(mat A, int colIndex, mat centroids){
int index=1;
double lowestDistance=getEuclideanDistance(A, colIndex, centroids, 0);
for(int i=1;i<centroids.n_cols;++i){
double curDistance=getEuclideanDistance(A, colIndex, centroids, i);
if(curDistance<lowestDistance){
lowestDistance=curDistance;
index=i+1;
}
}
return index;
}
static double getEuclideanDistance(mat A, int colIndexA, mat B, int colIndexB){
double distance=0;
for(int i=0;i<A.n_rows;++i){
double elementA=A(i,colIndexA);
double elementB=B(i,colIndexB);
double diff=elementA-elementB;
distance+=diff*diff;
}
return sqrt(distance);
}
static mat getSqrtMat(mat A){
cx_mat result=sqrtmat(A);
return real(result);
}
static mat invertDiagMatrix(mat A){
for(int i=0;i<A.n_rows;++i){
double curVal = A(i,i);
A(i,i) = 1/curVal;
}
return A;
}
};
#endif
#ifndef TESTS_A_COMPA
#define TESTS_A_COMPA
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
using namespace arma;
class tests_a_compA{
public:
double rosIn;
double noRosIn;
double rosOut;
double noRosOut;
void init()
{
}
void execute()
{
rosOut = rosIn;
}
};
#endif
#pragma once
#include "tests_a_compA.h"
#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>
#include <std_msgs/Float32.h>
class tests_a_compA_RosWrapper{
tests_a_compA component;
ros::Subscriber _clockSubscriber;
ros::Publisher _echoPublisher;
public:
void _clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg){
component.rosIn = msg->clock.toSec();
ROS_INFO("%f", msg->clock.toSec());
}
tests_a_compA_RosWrapper(ros::NodeHandle node_handle, ros::NodeHandle private_node_handle){
_clockSubscriber = node_handle.subscribe("/clock" ,5,&tests_a_compA_RosWrapper::_clockCallback, this, ros::TransportHints().tcpNoDelay());
_echoPublisher = node_handle.advertise<std_msgs::Float32>("/echo",5);
}
void publish0(){
std_msgs::Float32 tmpMsg;
tmpMsg.data = component.rosOut;
_echoPublisher.publish(tmpMsg);
}
void tick(){
component.execute();
publish0();
}
};
/*
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include <ros/ros.h>
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include <std_msgs/String.h>
// %EndTag(MSG_HEADER)%
#include "tests_a_compA_RosWrapper.h"
#include <sstream>
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
// %Tag(INIT)%
ros::init(argc, argv, "torcs");
// %EndTag(INIT)%
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
// %Tag(NODEHANDLE)%
ros::NodeHandle n;
// %EndTag(NODEHANDLE)%
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
// %Tag(PUBLISHER)%
// ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
// %EndTag(PUBLISHER)%
// %Tag(LOOP_RATE)%
ros::Rate loop_rate(10);
// %EndTag(LOOP_RATE)%
tests_a_compA_RosWrapper testsWrapper(n, n);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
// %Tag(ROS_OK)%
int count = 0;
while (ros::ok())
{
// %EndTag(ROS_OK)%
/**
* This is a message object. You stuff it with data, and then publish it.
*/
// %Tag(FILL_MESSAGE)%
//std_msgs::String msg;
//std::stringstream ss;
//ss << "hello world " << count;
//msg.data = ss.str();
// %EndTag(FILL_MESSAGE)%
// %Tag(ROSCONSOLE)%
//ROS_INFO("%s", msg.data.c_str());
ROS_INFO("Tick");
// %EndTag(ROSCONSOLE)%
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
// %Tag(PUBLISH)%
//chatter_pub.publish(msg);
// %EndTag(PUBLISH)%
// %Tag(SPINONCE)%
//ros::spinOnce();
// %EndTag(SPINONCE)%
testsWrapper.tick();
// %Tag(RATE_SLEEP)%
loop_rate.sleep();
// %EndTag(RATE