Commit c9247de8 authored by David Tiede's avatar David Tiede
Browse files

Initial commit

parents
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);
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