Skip to content
Snippets Groups Projects
Commit c9247de8 authored by David Tiede's avatar David Tiede
Browse files

Initial commit

parents
No related branches found
No related tags found
No related merge requests found
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
<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
<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
#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;
}
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
<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
<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>
#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);
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment