Skip to content
Snippets Groups Projects
Select Git revision
  • cb0f35a0e2571269fcaedab3756fecee57e8a0db
  • dev default protected
  • trace-incremental
3 results

index.php

Blame
  • 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;
      }
    }