Select Git revision
robot_control_node.cpp 7.19 KiB
#include <jsoncpp/json/json.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <ros/ros.h>
#include <boost/filesystem.hpp>
#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing
#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
static const std::string PLANNING_GROUP_ARM = "panda_arm";
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 collision_object;
collision_object.header.frame_id = "world";
collision_object.id = name;
const Json::Value dimensions = root["dimensions"];
ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = dimensions["x"].asDouble();
primitive.dimensions[1] = dimensions["y"].asDouble();
primitive.dimensions[2] = dimensions["z"].asDouble();
const Json::Value position = root["position"];
ROS_INFO_STREAM("Extracted position: " << position);
const Json::Value orientation = root["orientation"];
ROS_INFO_STREAM("Extracted orientation: " << orientation);
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = orientation["w"].asDouble();
box_pose.orientation.x = orientation["x"].asDouble();
box_pose.orientation.y = orientation["y"].asDouble();
box_pose.orientation.z = orientation["z"].asDouble();
// MoveIt! planning scene expects the center of the object as position.
// We add half of its dimension to its position
box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
spawnCollisionBox(primitive, box_pose);
return std::move(collision_object);
}
int main(int argc, char **argv)
{
namespace fs = boost::filesystem;
ROS_INFO("RUNNING robot_control_node");
ros::init(argc, argv, "robot_control_node");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
sleep_t.sleep();
}
moveit_msgs::PlanningScene planning_scene;
// read JSON files from ~/.panda_simulation
fs::path home(getenv("HOME"));
if (fs::is_directory(home))
{
fs::path app_directory(home);
app_directory /= APP_DIRECTORY_NAME;
if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
{
ROS_WARN_STREAM(app_directory << " does not exist");
// Create .panda_simulation directory
std::string path(getenv("HOME"));
path += "/.panda_simulation";
ROS_INFO("Creating %s collision objects directory.", path);
try
{
boost::filesystem::create_directory(path);
}
catch (const std::exception&)
{
ROS_ERROR(
"%s directory could not be created."
"Please create this directory yourself "
"if you want to specify collision objects.", path.c_str());
return -1;
}
}
std::vector<moveit_msgs::CollisionObject> collision_objects;
ROS_INFO_STREAM(app_directory << " is a directory containing:");
for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
{
ROS_INFO_STREAM(entry);
std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
if (file_stream)
{
Json::Value root;
file_stream >> root;
moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
collision_objects.push_back(collision_object);
}
else
{
ROS_WARN_STREAM("could not open file " << entry.path());
}
}
// Publish the collision objects to the scene
for (const auto &collision_object : collision_objects)
{
collision_object.header.frame_id = move_group_arm.getPlanningFrame();
planning_scene.world.collision_objects.push_back(collision_object);
}
ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
ROS_INFO("robot_control_node is ready");
ros::waitForShutdown();
return 0;
}
}