Changed directory structure, added existing files

BasJ93 · Oct 16, 2017 · f7a3f74 · f7a3f74
1 parent c9e534c
commit f7a3f74
Show file tree

Hide file tree

Showing 9 changed files with 2,582 additions and 0 deletions.
diff --git a/ar_por/CMakeLists.txt b/ar_por/CMakeLists.txt
@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ar_por)
+
+## 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
+  descartes_core
+  descartes_moveit
+  descartes_trajectory
+  descartes_planner
+  geometry_msgs
+  trajectory_msgs
+  visualization_msgs
+  eigen_conversions
+  tf
+)
+find_package(Eigen REQUIRED)
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+###################################
+## 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 descartes_core descartes_moveit descartes_planner
+  CATKIN_DEPENDS descartes_core descartes_moveit descartes_planner geometry_msgs roscpp tf trajectory_msgs visualization_msgs descartes_trajectory
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  include
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ar_por.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}_arm src/Alfa.cpp src/ar_por_arm.cpp)
+ add_executable(${PROJECT_NAME}_arm_graph src/Alfa.cpp src/ar_por_arm_graph.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+ add_dependencies(${PROJECT_NAME}_arm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+ add_dependencies(${PROJECT_NAME}_arm_graph ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(${PROJECT_NAME}_arm ${catkin_LIBRARIES})
+ target_link_libraries(${PROJECT_NAME}_arm_graph ${catkin_LIBRARIES})
+
+## 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}_arm PROPERTIES OUTPUT_NAME arm_irb120 PREFIX "")
+ set_target_properties(${PROJECT_NAME}_arm_graph PROPERTIES OUTPUT_NAME arm_irb120_graph PREFIX "")
+
+#############
+## 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_ar_por.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)
diff --git a/ar_por/include/Alfa.h b/ar_por/include/Alfa.h
@@ -0,0 +1,34 @@
+#include <math.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <descartes_planner/dense_planner.h>
+
+#ifndef ALFA_H_
+#define ALFA_H_
+
+EigenSTL::vector_Affine3d createA(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createB(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createC(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createD(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createE(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createF(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createG(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createH(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createI(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createJ(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createK(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createL(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createM(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createN(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createO(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createP(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createQ(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createR(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createS(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createT(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createU(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createV(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createW(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createX(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createY(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+EigenSTL::vector_Affine3d createZ(EigenSTL::vector_Affine3d points, float safeOffset, float xOffset, float yOffset, float step);
+#endif
diff --git a/ar_por/launch/arm_irb120.launch b/ar_por/launch/arm_irb120.launch
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="sim" default="true"/>
+  <include file="$(find abb_irb120_moveit_config)/launch/moveit_planning_execution.launch">
+    <arg name="sim" value="$(arg sim)"/>
+  </include>
+  <node name="arm_irb120" pkg="ar_por" type="arm_irb120" output="screen" />
+
+</launch>
diff --git a/ar_por/launch/arm_irb120_graph.launch b/ar_por/launch/arm_irb120_graph.launch
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="sim" default="true"/>
+  <include file="$(find abb_irb120_moveit_config)/launch/moveit_planning_execution.launch">
+    <arg name="sim" value="$(arg sim)"/>
+  </include>
+  <node name="arm_irb120_graph" pkg="ar_por" type="arm_irb120_graph" output="screen" />
+
+</launch>
diff --git a/ar_por/package.xml b/ar_por/package.xml
@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package>
+  <name>ar_por</name>
+  <version>0.0.0</version>
+  <description>The ar_por package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">bas</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/ar_por</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">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>descartes_core</build_depend>
+  <build_depend>descartes_moveit</build_depend>
+  <build_depend>descartes_trajectory</build_depend>
+  <build_depend>descartes_planner</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+
+  <run_depend>descartes_core</run_depend>
+  <run_depend>descartes_moveit</run_depend>
+  <run_depend>descartes_trajectory</run_depend>
+  <run_depend>descartes_planner</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/ar_por/paths/a.path b/ar_por/paths/a.path
@@ -0,0 +1,52 @@
+0.000000000	-0.289927	0.0234536	0.349743	1.46245	1.84025	4.32449	
+0.2000000000	-0.284989	0.0645015	0.405752	1.43882	1.82413	4.2253	
+0.4000000000	-0.28021	0.116152	0.450922	1.41742	1.80623	4.12714	
+0.6000000000	-0.275585	0.178043	0.48505	1.39846	1.78692	4.03055	
+0.8000000000	-0.271107	0.249555	0.507902	1.3821	1.76662	3.93634	
+1.0000000000	-0.266769	0.329778	0.519282	1.36846	1.74586	3.84551	
+1.2000000000	-0.262562	0.417541	0.519081	1.35759	1.7252	3.75921	
+1.4000000000	-0.258487	0.511515	0.507291	1.34935	1.70524	3.67861	
+1.6000000000	-0.254532	0.610295	0.484038	1.34359	1.68654	3.60484	
+1.8000000000	-0.250697	0.712563	0.449521	1.33999	1.66955	3.53885	
+2.0000000000	-0.246966	0.817205	0.403964	1.33822	1.65463	3.48142	
+2.2000000000	-0.246966	0.817205	0.403964	1.33822	1.65463	3.48142	
+2.4000000000	-0.246966	1.14765	0.253717	1.32723	1.61203	3.30598	
+2.6000000000	-0.264264	1.16615	0.198665	1.41457	1.6033	3.34503	
+2.8000000000	-0.275166	1.20223	0.0977897	1.40645	1.61618	3.40863	
+3.0000000000	-0.284953	1.24362	-0.0104102	1.40053	1.6302	3.47411	
+3.2000000000	-0.288832	1.31308	-0.180216	1.30792	1.69188	3.5635	
+3.4000000000	-0.301786	1.34597	-0.256939	1.39568	1.66164	3.61538	
+3.6000000000	-0.31305	1.35463	-0.276809	1.57177	1.57027	3.63457	
+3.8000000000	-0.318882	1.39109	-0.359275	1.65669	1.51954	3.67837	
+4.0000000000	-0.322441	1.43205	-0.44973	1.73881	1.45966	3.72071	
+42.000000000	-0.317344	1.44608	-0.480264	1.91907	1.33905	3.70563	
+44.000000000	-0.315356	1.49841	-0.592324	1.99629	1.25794	3.73819	
+46.000000000	-0.354507	1.47034	-0.53255	1.88109	1.35039	3.73999	
+48.000000000	-0.387392	1.41738	-0.417569	1.86498	1.38665	3.68522	
+50.000000000	-0.422289	1.37298	-0.31856	1.84314	1.41925	3.63716	
+52.000000000	-0.463667	1.36008	-0.289271	1.71559	1.49214	3.63587	
+54.000000000	-0.503682	1.3502	-0.266671	1.5884	1.56147	3.62877	
+56.000000000	-0.542128	1.34258	-0.249114	1.46121	1.6273	3.61582	
+58.000000000	-0.583248	1.31298	-0.179983	1.42169	1.64022	3.5742	
+60.000000000	-0.62108	1.30915	-0.170907	1.29062	1.69779	3.55622	
+62.000000000	-0.657066	1.30613	-0.163749	1.15902	1.75159	3.53215	
+64.000000000	-0.702092	1.28447	-0.111721	1.11129	1.75517	3.4964	
+66.000000000	-0.702092	0.989143	0.0430539	1.13899	1.81584	3.62619	
+68.000000000	-0.290283	1.11085	-0.203402	1.25341	1.81002	3.76648	
+70.000000000	-0.296911	1.37162	-0.315476	1.31052	1.71531	3.6373	
+72.000000000	-0.341699	1.36337	-0.296764	1.3624	1.68446	3.63388	
+74.000000000	-0.38051	1.38384	-0.34303	1.3313	1.70889	3.65493	
+76.000000000	-0.418153	1.40779	-0.396416	1.3027	1.73516	3.6788	
+78.000000000	-0.454578	1.4358	-0.457919	1.27696	1.76352	3.7059	
+80.000000000	-0.495939	1.43621	-0.458806	1.33125	1.72948	3.71585	
+82.000000000	-0.489751	1.1791	-0.339429	1.2778	1.82429	3.83512	
+84.000000000	-0.489751	1.1791	-0.339429	1.2778	1.82429	3.83512	
+86.000000000	-0.477703	1.03611	-0.178554	1.28309	1.81154	3.8198	
+88.000000000	-0.464591	0.901907	-0.0449667	1.29372	1.80346	3.82286	
+90.000000000	-0.450271	0.773024	0.0673969	1.30879	1.79874	3.84181	
+92.000000000	-0.434577	0.647803	0.161545	1.32787	1.79619	3.87541	
+94.000000000	-0.417311	0.52568	0.238929	1.35069	1.79459	3.92295	
+96.000000000	-0.398234	0.406892	0.300152	1.37693	1.79257	3.98369	
+98.000000000	-0.369007	0.322734	0.306217	1.3471	1.86677	4.04995	
+100.000000000	-0.34555	0.215091	0.336963	1.3842	1.86335	4.13277	
+102.000000000	-0.319348	0.114727	0.351498	1.42324	1.85505	4.22501