Skip to content
Snippets Groups Projects
Commit 13bcc556 authored by Johannes Mey's avatar Johannes Mey
Browse files

add safety zone spawner node, improve and rename config file

parent 6756d666
No related branches found
No related tags found
No related merge requests found
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.0.2)
project(panda_mqtt_connector)
add_compile_options(-std=c++14)
set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS} -std=c++14")
# 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 controller_manager
find_package(catkin REQUIRED COMPONENTS
controller_manager
effort_controllers
gazebo_ros
genmsg
......@@ -25,7 +24,9 @@ find_package(catkin REQUIRED
moveit_ros_planning
moveit_ros_planning_interface
controller_interface
hardware_interface)
hardware_interface
simulation_util
)
# System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS filesystem)
......@@ -33,14 +34,12 @@ find_package(Boost REQUIRED COMPONENTS filesystem)
find_package(Protobuf 3 REQUIRED)
include_directories(${Protobuf_INCLUDE_DIRS})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto proto/linkstate.proto proto/robotconfig.proto proto/trajectory.proto)
SET_SOURCE_FILES_PROPERTIES(${PROTO_SRC} ${PROTO_HDRS} PROPERTIES GENERATED TRUE)
message("psource " ${PROTO_SRC})
message("pheaders" ${PROTO_HDRS})
find_package(PkgConfig REQUIRED)
......@@ -52,12 +51,6 @@ set(PAHO_MQTT_C_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/lib/paho.mqtt.c/src")
add_subdirectory(lib/paho.mqtt.cpp)
get_cmake_property(_variableNames VARIABLES)
list (SORT _variableNames)
foreach (_variableName ${_variableNames})
message(STATUS "${_variableName}=${${_variableName}}")
endforeach()
# ################################################################################################################################
# catkin specific configuration ##
# ################################################################################################################################
......@@ -69,6 +62,7 @@ catkin_package(CATKIN_DEPENDS
controller_interface
hardware_interface
pluginlib
simulation_util
DEPENDS
# system_lib
)
......@@ -79,7 +73,6 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(
include
lib/paho.mqtt.c/src
lib/paho.mqtt.cpp/src
${catkin_INCLUDE_DIRS}
......@@ -90,14 +83,23 @@ add_dependencies(paho-mqttpp3 paho-mqtt3c)
add_library(TrajectoryUtil src/util/TrajectoryUtil.cpp src/util/TrajectoryUtil.h)
add_library(MqttUtil src/util/MqttUtil.cpp src/util/MqttUtil.h)
# Specify libraries to link a library or executable target against
target_link_libraries(TrajectoryUtil ${catkin_LIBRARIES})
add_executable(RobotStateProvider src/RobotStateProvider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(TimedPlanner src/TimedPlanner.cpp)
add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp)
set_target_properties(${PROJECT_NAME}_safety_zone_spawner PROPERTIES OUTPUT_NAME safety_zone_spawner PREFIX "")
add_dependencies(${PROJECT_NAME}_safety_zone_spawner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# Specify libraries to link a library or executable target against
target_link_libraries(TrajectoryUtil
${catkin_LIBRARIES}
)
target_link_libraries(RobotStateProvider LINK_PUBLIC
MqttUtil
${catkin_LIBRARIES}
......@@ -117,3 +119,5 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
${Protobuf_LIBRARIES}
)
target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
Copyright 2019 Erdal Pekel
Copyright 2020 Technische Universität Dresden, Software Technology Group
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
......
panda_mqtt_connector:
server: "tcp://localhost:1883"
topics:
- robotConfig: "robotconfig"
- dataConfig: "dataconfig"
zone_size: 0.5
zones:
- "1 1"
- "-1 -1 1"
joints:
- panda/Joint0: "panda::panda_link0"
- panda/Joint1: "panda::panda_link1"
- panda/Joint2: "panda::panda_link2"
- panda/Joint3: "panda::panda_link3"
- panda/Joint4: "panda::panda_link4"
- panda/Joint5: "panda::panda_link5"
- panda/Joint6: "panda::panda_link6"
- panda/LeftFinger: "panda::panda_leftfinger"
- panda/RightFinger: "panda::panda_rightfinger"
end_effectors:
- panda/EndEffector: "panda::panda_link7"
\ No newline at end of file
panda_mqtt_connector:
server: "tcp://localhost:1883"
safety_zone_spawner:
size: 0.5
zones:
- "1 1"
- "-1 -1 1"
\ No newline at end of file
......@@ -3,11 +3,11 @@
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<!-- the launch file parameter mqtt_server overrides the config file parameter -->
<rosparam file="$(find panda_mqtt_connector)/config/mqtt.yaml" command="load" />
<rosparam file="$(find panda_mqtt_connector)/config/config.yaml" command="load" />
<rosparam unless="$(eval arg('mqtt_server') == '')" param="panda_mqtt_connector/server" subst_value="True">$(arg mqtt_server)</rosparam>
<node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
<node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
<node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
<node name="gazebo_zones" pkg="gazebo_zones" type="gazebo_zones" respawn="false" output="screen"/>
<node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
</launch>
<launch>
<arg name="mqtt_server" default=""/>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<!-- the launch file parameter mqtt_server overrides the config file parameter -->
<rosparam file="$(find panda_mqtt_connector)/config/config.yaml" command="load" />
<rosparam unless="$(eval arg('mqtt_server') == '')" param="panda_mqtt_connector/server" subst_value="True">$(arg mqtt_server)</rosparam>
<node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
<node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
<node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
<node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
<node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
<node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
</launch>
/**
\mainpage
\htmlinclude manifest.html
## Provided Nodes
### safety_zone_spawner
* **Author:** Johannes Mey*
This node spawns cubic safety zones in gazebo.
It expects the ROS parameter `/safety_zone_spawner/zone_size` and `/safety_zone_spawner/zones` to be set.
- `/safety_zone_spawner/zone_size: double`
the edge size of a safety zone cube to be displayed in *m*.
- `/safety_zone_spawner/zones: std::vector<std::string>`
a list of safety zone cube integer coordinates. The coordinates are encoded in a string and separated by spaces
and have an `x`, a `y`, and and optionally a `z` coordinate. Examples are `"3 0"` or `"-5 2 4"`. The coordinates
are interpreted as follows: the cube at position "0 0 0" is centered at the origin of the coordinate system in
`x` and `y` direction, but is located *on top of the x-y-plane*. All other node coordinates are defined such that
they completely fill the space. This means that the actual coordinates (in *m*) can be computed using the ones
given here and the `zone_size`.
*/
......@@ -42,6 +42,7 @@
<depend>moveit_visual_tools</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>simulation_util</depend>
<exec_depend>pluginlib</exec_depend>
......
/*! \file safety_zone_spawner.cpp
\brief ROS node main file that spawns safety zones in gazebo.
\author Johannes Mey
\date 28.06.20
*/
#include <ros/ros.h>
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
#include <GazeboZoneSpawner.h>
/**
* The main method of the ROS node
*
* It does not requite command-line parameters, but expects the ROS parameter `/safety_zone_spawner/zone_size` and
* `/safety_zone_spawner/zones` to be set.
*
* - `/safety_zone_spawner/zone_size: double`
*
* the edge size of a safety zone cube to be displayed in *m*.
* - `/safety_zone_spawner/zones: std::vector<std::string>`
*
* a list of safety zone cube integer coordinates. The coordinates are encoded in a string and separated by spaces
* and have an `x`, a `y`, and and optionally a `z` coordinate. Examples are `"3 0"` or `"-5 2 4"`. The coordinates
* are interpreted as follows: the cube at position "0 0 0" is centered at the origin of the coordinate system in
* `x` and `y` direction, but is located *on top of the x-y-plane*. All other node coordinates are defined such that
* they completely fill the space. This means that the actual coordinates (in *m*) can be computed using the ones
* given here and the `zone_size`.
*/
int main(int argc, char **argv) {
// setup this ros-node
ros::init(argc, argv, "safety_zone_spawner");
ros::NodeHandle node_handle("panda_mqtt_connector");
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for gazebo
// TODO this should actually wait until gazebo has started
ros::Duration(2.0).sleep();
std::vector<std::string> keys;
node_handle.getParamNames(keys);
for (auto key : keys) {
ROS_ERROR_STREAM(key);
}
// get size
double size = 0.5;
if (!node_handle.getParam("zone_size", size)) {
ROS_WARN_STREAM("Could not get string value for " << node_handle.getNamespace() << "/size from param server, defaulting to " << size);
}
shape_msgs::SolidPrimitive box;
box.type = shape_msgs::SolidPrimitive::BOX;
box.dimensions = {size, size, size};
// get list of zone voxels
std::vector<std::string> voxels;
if (!node_handle.getParam("zones", voxels)) {
ROS_WARN_STREAM("Could not get string value for " << node_handle.getNamespace() << "/zones from param server. Not adding any zones.");
}
for (const std::string &voxel: voxels) {
geometry_msgs::Pose pose;
std::stringstream stream(voxel);
int x, y, z = 0;
stream >> x >> y;
if (stream) {
stream >> z; // the z value is 0 by default
}
pose.position.x = x * size;
pose.position.y = y * size;
pose.position.z = z * size + size / 2;
// the orientation is always the default
pose.orientation.w = 1;
GazeboZoneSpawner::spawnCollisionBox(box, pose);
}
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment