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

extract Gazebo safety zone spawner to library

parent 21f63d8d
Branches
No related tags found
No related merge requests found
...@@ -60,7 +60,7 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS}) ...@@ -60,7 +60,7 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
# add custom controller as library # add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp) add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
#add_library(SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp) add_library(GazeboZoneSpawner src/GazeboZoneSpawner.cpp src/GazeboZoneSpawner.h)
# Specify libraries to link a library or executable target against # Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
...@@ -71,9 +71,10 @@ add_executable(box_publisher_node src/box_publisher.cpp) ...@@ -71,9 +71,10 @@ add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp) add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp) add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_LIBRARIES}) target_link_libraries(GazeboZoneSpawner ${catkin_LIBRARIES} ${SDF_LIBRARIES})
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_LIBRARIES} GazeboZoneSpawner)
target_link_libraries(box_publisher_node ${catkin_LIBRARIES}) target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES}) target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES}) target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator) # target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
//
// Created by Johannes Mey on 31.03.20.
//
#include "GazeboZoneSpawner.h"
#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing
#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
#include <ros/ros.h>
#include <ros/package.h>
void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
if (shape.type != shape.BOX) {
ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
return;
}
sdf::SDFPtr sdf(new sdf::SDF());
sdf::init(sdf);
auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
assert(sdf::readFile(sdfFileName, sdf));
auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
auto poseElement = visualElement->GetElement("pose");
auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
std::ostringstream poseStream;
poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " 0 0 0";
poseElement->Set(poseStream.str());
ROS_INFO_NAMED("GazeboZoneSpawner", "Set pose in SDF file to '%s'.", poseElement->GetValue()->GetAsString().c_str());
std::ostringstream sizeStream;
sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
sizeElement->Set(sizeStream.str());
ROS_INFO_NAMED("GazeboZoneSpawner", "Set size in SDF file to '%s'.", sizeElement->GetValue()->GetAsString().c_str());
// FIXME orientation is currently ignored
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
gazebo_msgs::SpawnModel spawnService;
spawnService.request.robot_namespace = "box space";
// spawnModelService.request.initial_pose = pose; // not required here
spawnService.request.model_name = sdf->Root()->GetElement("model")->GetAttribute("name")->GetAsString();
spawnService.request.model_xml = sdf->ToString().c_str();
// spawnModelService.request.reference_frame = ; // if left empty, world is used
if (client.call(spawnService)) {
if ( spawnService.response.success) {
ROS_INFO_NAMED("GazeboZoneSpawner", "Successfully spawned Box. %s", spawnService.response.status_message.c_str());
} else {
ROS_INFO_NAMED("GazeboZoneSpawner", "Failed to spawn Box. %s", spawnService.response.status_message.c_str());
}
} else {
ROS_ERROR_NAMED("GazeboZoneSpawner", "Failed to call service '/gazebo/spawn_sdf_model'");
}
}
//
// Created by Johannes Mey on 31.03.20.
//
#ifndef PANDA_SIMULATION_GAZEBOZONESPAWNER_H
#define PANDA_SIMULATION_GAZEBOZONESPAWNER_H
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
class GazeboZoneSpawner {
public:
static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
};
#endif //PANDA_SIMULATION_GAZEBOZONESPAWNER_H
...@@ -6,62 +6,12 @@ ...@@ -6,62 +6,12 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <boost/filesystem.hpp> #include <boost/filesystem.hpp>
#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing #include "GazeboZoneSpawner.h"
#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
static const std::string PLANNING_GROUP_ARM = "panda_arm"; static const std::string PLANNING_GROUP_ARM = "panda_arm";
static const std::string APP_DIRECTORY_NAME = ".panda_simulation"; static const std::string APP_DIRECTORY_NAME = ".panda_simulation";
void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
if (shape.type != shape.BOX) {
ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
return;
}
sdf::SDFPtr sdf(new sdf::SDF());
sdf::init(sdf);
auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
assert(sdf::readFile(sdfFileName, sdf));
ROS_INFO_NAMED("model-loader", "successfully read SDF file: %s", sdf->ToString().c_str());
auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
auto poseElement = visualElement->GetElement("pose");
auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
std::ostringstream poseStream;
poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " 0 0 0";
poseElement->Set(poseStream.str());
ROS_INFO_NAMED("model-loader", "successfully changed SDF file, pose is: %s", poseElement->GetValue()->GetAsString().c_str());
std::ostringstream sizeStream;
sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
sizeElement->Set(sizeStream.str());
ROS_INFO_NAMED("model-loader", "successfully changed SDF file, size is: '%s', should be '%s'", sizeElement->GetValue()->GetAsString().c_str(), sizeStream.str().c_str());
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
gazebo_msgs::SpawnModel spawnModelService;
spawnModelService.request.robot_namespace = "box space";
// spawnModelService.request.initial_pose = pose; // not required here
spawnModelService.request.model_name = sdf->Root()->GetElement("model")->GetAttribute("name")->GetAsString();
spawnModelService.request.model_xml = sdf->ToString().c_str();
// spawnModelService.request.reference_frame = ; // if left empty, world is used
if (client.call(spawnModelService))
{
ROS_INFO("result: %s", spawnModelService.response.success ? "success!" : "failed!");
ROS_INFO("result: %s", spawnModelService.response.status_message.c_str());
}
else
{
ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
}
}
moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name) moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
{ {
...@@ -101,7 +51,7 @@ moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::str ...@@ -101,7 +51,7 @@ moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::str
collision_object.primitive_poses.push_back(box_pose); collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD; collision_object.operation = collision_object.ADD;
spawnCollisionBox(primitive, box_pose); GazeboZoneSpawner::spawnCollisionBox(primitive, box_pose);
return std::move(collision_object); return std::move(collision_object);
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment