ROS Bridge Overview and Workflow

            

            
roscpp rospy actionlib_msgs control_msgs diagnostic_msgs geometry_msgs
map_msgs nav_msgs pcl_msgs sensor_msgs shape_msgs std_msgs stereo_msgs
tf2_geometry_msgs tf2_msgs trajectory_msgs visualization_msgs
        

            

            
/*
Copyright (c) 2018, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/
#pragma once

#include <memory>
#include <string>

#include "engine/alice/alice.hpp"
#include "messages/messages.hpp"

namespace isaac {
namespace rosbridge {

// This codelet represents a basic bridge to ROS for communicating navigation events.
// It receives Pose2D messages from ROS and emits goal protos
// It will also check the current pose of the robot and publish Pose2D messages to ROS
class NavigationRosBridge : public alice::Codelet {
 public:
  // Explicitly declare constructors and destructors
  // to get around forward declaration of RosNavigationData
  NavigationRosBridge();
  virtual ~NavigationRosBridge();

  void start() override;
  void tick() override;
  void stop() override;

  // Output goal for the robot
  ISAAC_PROTO_TX(Goal2Proto, goal);

  // Output pose where the estimated pose of the robot is written
  ISAAC_POSE2(world, robot);

  // ROS publisher queue depth
  ISAAC_PARAM(int, publisher_queue_size, 1000);
  // ROS subscriber queue depth
  ISAAC_PARAM(int, subscriber_queue_size, 1000);
  // ROS publisher channel. Used to broadcast messages to ROS
  ISAAC_PARAM(std::string, publisher_channel_name, "isaac_navigation2D_status");
  // ROS subscriber channel. Used to receive messagse from ROS
  ISAAC_PARAM(std::string, subscriber_channel_name, "isaac_navigation2D_request");

 private:
  // Hide the ROS implementation details
  struct RosNavigationData;
  std::unique_ptr<RosNavigationData> nav_data_;
};

}  // namespace rosbridge
}  // namespace isaac

ISAAC_ALICE_REGISTER_CODELET(isaac::rosbridge::NavigationRosBridge);
        

            

            
/*
Copyright (c) 2018, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/
#include "NavigationRosBridge.hpp"

#include <memory>

#include "engine/core/assert.hpp"
#include "geometry_msgs/Pose2D.h"
#include "messages/math.hpp"
#include "ros/callback_queue.h"
#include "ros/ros.h"

namespace isaac {
namespace rosbridge {

namespace {
// Callback functor to avoid having a callback function in the bridge class
class CallbackFunctor {
 public:
  explicit CallbackFunctor(NavigationRosBridge* bridge) {
    bridge_ = bridge;
  }
  CallbackFunctor(const CallbackFunctor&) = default;
  ~CallbackFunctor() = default;

  void operator()(const geometry_msgs::Pose2D::ConstPtr& msg) {
    const Pose2d pose = Pose2d::FromXYA(msg->x, msg->y, msg->theta);
    auto goal_proto = bridge_->tx_goal().initProto();
    ToProto(pose, goal_proto.initGoal());
    goal_proto.setGoalFrame("world");
    bridge_->tx_goal().publish();
  }
 private:
  NavigationRosBridge* bridge_;
};
}  // namespace

// Internal struct for holding the ROS node handle and the publisher and subscriber channels
// Note the callback queue. To avoid spinning other codelets it is necessary to generate
// a separate callback queue per codelet.
struct NavigationRosBridge::RosNavigationData {
  ros::NodeHandle node;
  ros::Subscriber sub;
  ros::Publisher pub;
  ros::CallbackQueue callbackQueue;
};

void NavigationRosBridge::start() {
  // Setup the ros node and communication channels
  // Make sure to disable the Sigint handler and if any args are needed
  // add them to the args string.
  ros::M_string args;
  if (!ros::isInitialized()) {
    ros::init(args, "isaacRosBridge", ros::init_options::NoSigintHandler);
  }

  nav_data_ = std::make_unique<RosNavigationData>();
  nav_data_->node.setCallbackQueue(&(nav_data_->callbackQueue));
  nav_data_->pub = nav_data_->node.advertise<geometry_msgs::Pose2D>(
      get_publisher_channel_name(), get_publisher_queue_size());
  nav_data_->sub = nav_data_->node.subscribe<geometry_msgs::Pose2D>(
      get_subscriber_channel_name(), get_subscriber_queue_size(), CallbackFunctor(this));

  // Because ROS requires that the message queues be manually pumped we need to tick.
  tickPeriodically();
}

void NavigationRosBridge::tick() {
  if (ros::ok()) {
    bool ok;
    const Pose2d world_T_robot = get_world_T_robot(getTickTime(), ok);
    if (ok) {
      geometry_msgs::Pose2D msg;
      msg.x = world_T_robot.translation(0);
      msg.y = world_T_robot.translation(1);
      msg.theta = world_T_robot.rotation.angle();
      nav_data_->pub.publish(msg);
    }
    // Pump the queue for this codelet.
    nav_data_->callbackQueue.callAvailable();
  } else {
    LOG_ERROR("An error has occurred within ROS.");
  }
}

void NavigationRosBridge::stop() {
  nav_data_->pub.shutdown();
  nav_data_->sub.shutdown();
  nav_data_ = nullptr;
}

NavigationRosBridge::~NavigationRosBridge() {
}

NavigationRosBridge::NavigationRosBridge() {
}

}  // namespace rosbridge
}  // namespace isaac
        

            

            
#include <ros/ros.h>
// Include the appropriate message headers for your test case
#include <geometry_msgs/Pose2D.h>

// Sending random points for navigation testing
#include <random>

// A simple call back to acknowledge receiving a message
void navigationCallback(const geometry_msgs::Pose2D::ConstPtr& msg) {
    ROS_INFO("Heard: Isaac is at X:%f, Y:%f, Theta: %f", msg->x, msg->y, msg->theta);
}

constexpr double pi = 3.14159265358979323846;

int main(int argc, char **argv) {
    // Select a random point to send as a goal pose
    std::random_device rd;
    std::mt19937_64 eng(rd());
    std::uniform_real_distribution<> dist_x(10, 30);
    std::uniform_real_distribution<> dist_y(10, 30);
    std::uniform_real_distribution<> dist_theta(-pi, pi);

    ros::init(argc, argv, "rosBridgeTest");

    ros::NodeHandle rosBridgeTestNode;

    // Channel to publish navigation requests to.
    // Replace with desired message and channel name for your test case
    ros::Publisher nav_pub = rosBridgeTestNode.advertise<geometry_msgs::Pose2D>("isaac_navigation2D_request", 1000);

    // Listen for pose/navigation updates.
    // Replace with desired message and channel name for your test case
    ros::Subscriber nav_sub = rosBridgeTestNode.subscribe("isaac_navigation2D_status", 1000, navigationCallback);

    // Send a message and check updates every ten seconds
    ros::Rate loop_rate(10);

    while (ros::ok()) {
        // Generate a random goal location
        geometry_msgs::Pose2D pose_msg;
        pose_msg.x = dist_x(eng);
        pose_msg.y = dist_y(eng);
        pose_msg.theta = dist_theta(eng);

        ROS_INFO("Sent: Move Isaac to  X:%f, Y:%f, Theta: %f", pose_msg.x, pose_msg.y, pose_msg.theta);
        nav_pub.publish(pose_msg);

        // Manually spin to check messages
        ros::spinOnce();

        loop_rate.sleep();
    }
    return 0;
}
        

            

            
isaac_new_http_archive(
    name = "isaac_ros_bridge_x86_64",
    build_file = clean_dep("//third_party:ros.BUILD"),
    sha256 = "b2a6c2373fe2f02f3896586fec0c11eea83dff432e65787f4fbc0ef82100070a",
    url = "https://developer.nvidia.com/isaac/download/third_party/ros_kinetic_x86_64.tar.gz",
)
        

            

            
isaac_new_local_repository(
    name='isaac_ros_bridge_x86_64',
    path='path to the workspace',
    build_file = clean_dep("//third_party:ros.BUILD"),
)