Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
David Tiede
adapter_pkgs
Commits
c9247de8
Commit
c9247de8
authored
Apr 15, 2021
by
David Tiede
Browse files
Initial commit
parents
Changes
8
Hide whitespace changes
Inline
Side-by-side
adapter_package/CMakeLists.txt
0 → 100644
View file @
c9247de8
cmake_minimum_required
(
VERSION 3.0.2
)
project
(
adapter_package
)
## 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
moveit_ros_planning_interface
)
## 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 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
# )
################################################
## 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 exec_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 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 adapter_package
# CATKIN_DEPENDS roscpp rospy 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}/adapter_package.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
(
adapter_package src/adapter_package.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
(
adapter_package PROPERTIES OUTPUT_NAME adapter_package 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
(
adapter_package
${
catkin_LIBRARIES
}
)
\ No newline at end of file
adapter_package/launch/adapter_package.launch
0 → 100644
View file @
c9247de8
<launch>
<!--<include file="$(find panda_simulation)/launch/simulation.launch"/>-->
<!--<include file="$(find panda_simulation)/launch/moveit_rviz.launch"/>-->
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<node pkg="adapter_package" type="adapter_package" name="adapter_package" respawn="false" output="screen"/>
</launch>
\ No newline at end of file
adapter_package/package.xml
0 → 100644
View file @
c9247de8
<package
format=
"3"
>
<name>
adapter_package
</name>
<version>
0.0.1
</version>
<description>
The adapter_package package
</description>
<maintainer
email=
"david.tiede@mailbox.tu-dresden.de"
>
David Tiede
</maintainer>
<license>
TODO
</license>
<buildtool_depend>
catkin
</buildtool_depend>
<depend>
roscpp
</depend>
<depend>
std_msgs
</depend>
<export>
</export>
</package>
\ No newline at end of file
adapter_package/src/adapter_package.cpp
0 → 100644
View file @
c9247de8
#include
"std_msgs/String.h"
#include
<moveit/move_group_interface/move_group_interface.h>
void
move_toCallback
(
const
std_msgs
::
String
::
ConstPtr
&
msg
)
{
ROS_INFO
(
"ADAPTER PACKAGE: Recevied message: '%s'"
,
msg
->
data
.
c_str
());
static
const
std
::
string
PLANNING_GROUP
=
"panda_arm"
;
moveit
::
planning_interface
::
MoveGroupInterface
group
(
PLANNING_GROUP
);
ros
::
Duration
(
3.0
).
sleep
();
group
.
setStartStateToCurrentState
();
group
.
setPlanningTime
(
5.0
);
// pose A
geometry_msgs
::
Pose
target_pose_1
=
group
.
getCurrentPose
().
pose
;
target_pose_1
.
position
.
z
=
0.4
;
target_pose_1
.
position
.
y
=
0.4
;
target_pose_1
.
position
.
x
=
0.8
;
target_pose_1
.
orientation
.
x
=
0
;
target_pose_1
.
orientation
.
y
=
0
;
target_pose_1
.
orientation
.
z
=
0
;
target_pose_1
.
orientation
.
w
=
1
;
// pose B
geometry_msgs
::
Pose
target_pose_2
=
group
.
getCurrentPose
().
pose
;
target_pose_2
.
position
.
z
=
0.4
;
target_pose_2
.
position
.
y
=
0.4
;
target_pose_2
.
position
.
x
=
0.8
;
target_pose_2
.
orientation
.
x
=
0
;
target_pose_2
.
orientation
.
y
=
0
;
target_pose_2
.
orientation
.
z
=
0
;
target_pose_2
.
orientation
.
w
=
1
;
std
::
vector
<
geometry_msgs
::
Pose
>
waypoints
;
std
::
string
strA
(
"A"
);
std
::
string
strB
(
"B"
);
if
(
strA
==
msg
->
data
)
{
waypoints
.
push_back
(
target_pose_1
);
}
else
if
(
strB
==
msg
->
data
)
{
waypoints
.
push_back
(
target_pose_2
);
}
else
{
ROS_WARN
(
"ADAPTER PACKAGE: Wrong message."
);
return
;
}
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
plan
;
moveit_msgs
::
RobotTrajectory
trajectory_msg
;
group
.
computeCartesianPath
(
waypoints
,
0.01
,
0.0
,
trajectory_msg
,
true
);
plan
.
trajectory_
=
trajectory_msg
;
group
.
execute
(
plan
);
ROS_INFO
(
"ADAPTER PACKAGE: Finished movement."
);
}
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"adapter_package"
);
ros
::
NodeHandle
node_handle
;
ros
::
AsyncSpinner
spinner
(
2
);
spinner
.
start
();
ros
::
Subscriber
sub
=
node_handle
.
subscribe
(
"move_to"
,
1000
,
move_toCallback
);
ROS_INFO
(
"ADAPTER PACKAGE: Listening to 'move_to'."
);
ros
::
waitForShutdown
();
return
0
;
}
safety_planning_request_adapters/CMakeLists.txt
0 → 100644
View file @
c9247de8
cmake_minimum_required
(
VERSION 3.0.2
)
project
(
safety_planning_request_adapters
)
find_package
(
catkin REQUIRED COMPONENTS
roscpp
std_msgs
moveit_ros_planning
trajectory_msgs
)
catkin_package
(
LIBRARIES
${
PROJECT_NAME
}
CATKIN_DEPENDS moveit_ros_planning trajectory_msgs
)
include_directories
(
${
catkin_INCLUDE_DIRS
}
)
add_library
(
${
PROJECT_NAME
}
src/add_safety.cpp
)
target_link_libraries
(
${
PROJECT_NAME
}
${
catkin_LIBRARIES
}
)
install
(
TARGETS
${
PROJECT_NAME
}
LIBRARY DESTINATION
${
CATKIN_PACKAGE_LIB_DESTINATION
}
)
install
(
DIRECTORY include/
${
PROJECT_NAME
}
/
DESTINATION
${
CATKIN_PACKAGE_INCLUDE_DESTINATION
}
)
install
(
FILES planning_request_adapters_plugin_description.xml
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
)
\ No newline at end of file
safety_planning_request_adapters/package.xml
0 → 100644
View file @
c9247de8
<package
format=
"3"
>
<name>
safety_planning_request_adapters
</name>
<version>
0.0.1
</version>
<description>
The safety_planning_request_adapters package
</description>
<maintainer
email=
"david.tiede@mailbox.tu-dresden.de"
>
David Tiede
</maintainer>
<license>
TODO
</license>
<buildtool_depend>
catkin
</buildtool_depend>
<depend>
moveit_ros_planning
</depend>
<depend>
roscpp
</depend>
<depend>
std_msgs
</depend>
<depend>
trajectory_msgs
</depend>
<export>
<moveit_core
plugin=
"${prefix}/planning_request_adapters_plugin_description.xml"
/>
</export>
</package>
\ No newline at end of file
safety_planning_request_adapters/planning_request_adapters_plugin_description.xml
0 → 100644
View file @
c9247de8
<library
path=
"libsafety_planning_request_adapters"
>
<class
name=
"safety_planning_request_adapters/AddSafety"
type=
"safety_planning_request_adapters::AddSafety"
base_class_type=
"planning_request_adapter::PlanningRequestAdapter"
>
<description>
</description>
</class>
</library>
safety_planning_request_adapters/src/add_safety.cpp
0 → 100644
View file @
c9247de8
#include
<moveit/planning_request_adapter/planning_request_adapter.h>
#include
<class_loader/class_loader.hpp>
#include
<ros/console.h>
namespace
safety_planning_request_adapters
{
class
AddSafety
:
public
planning_request_adapter
::
PlanningRequestAdapter
{
int
VELOCITY_LIMIT
=
3
;
public:
std
::
string
getDescription
()
const
override
{
return
"Add Safety"
;
}
bool
adaptAndPlan
(
const
PlannerFn
&
planner
,
const
planning_scene
::
PlanningSceneConstPtr
&
planning_scene
,
const
planning_interface
::
MotionPlanRequest
&
req
,
planning_interface
::
MotionPlanResponse
&
res
,
std
::
vector
<
std
::
size_t
>&
added_path_index
)
const
override
{
ROS_ERROR
(
"ADAPTER PACKAGE: ADAPTER LOADED"
);
bool
result
=
planner
(
planning_scene
,
req
,
res
);
if
(
result
&&
res
.
trajectory_
)
// Only run after planning
{
// ROS_DEBUG("Running '%s'", getDescription().c_str());
// if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor,
// req.max_acceleration_scaling_factor))
// {
// ROS_ERROR("Time parametrization for the solution path failed.");
// result = false;
// }
}
return
result
;
}
};
}
// namespace safety_planning_request_adapters
CLASS_LOADER_REGISTER_CLASS
(
safety_planning_request_adapters
::
AddSafety
,
planning_request_adapter
::
PlanningRequestAdapter
);
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment