Permalink
Browse files

first working prototype for rosbridge working in the direction from R…

…OS to madara
  • Loading branch information...
auerj committed Jun 1, 2018
2 parents 01882f3 + d548774 commit d74cc85c332b160a2f286c6e74085c90ca13703a
@@ -14,6 +14,10 @@ feature (ros) {
ROS_Utility {
src/gams/utility/ros
}
ROS_Transport {
src/gams/transports/ros
}
}
Source_Files {
@@ -24,5 +28,9 @@ feature (ros) {
ROS_Utility {
src/gams/utility/ros
}
ROS_Transport {
src/gams/transports/ros
}
}
}
@@ -482,7 +482,7 @@ if [ $ACE -eq 1 ] || [ $ACE_AS_A_PREREQ -eq 1 ]; then
echo "ENTERING $MPC_ROOT"
# build ACE, all build information (compiler and options) will be set here
if [ ! -d $MPC_ROOT ] ; then
git clone --depth 1 https://github.com/DOCGroup/MPC.git
git clone --depth 1 https://github.com/DOCGroup/MPC.git $MPC_ROOT
MPC_REPO_RESULT=$?
fi
@@ -101,7 +101,7 @@ bool differential_checkpoints = false;
int save_checkpoint (knowledge::KnowledgeBase *knowledge,
knowledge::CheckpointSettings *settings, std::string meta_prefix="meta");
std::string get_agent_var_prefix (std::string ros_topic_name);
std::string ros_to_gams_name (std::string topic_name);
std::string gams::utility::ros::ros_to_gams_name (std::string topic_name);
// handle command line arguments
@@ -310,7 +310,7 @@ int main (int argc, char ** argv)
else
{
container_name = get_agent_var_prefix (topic) + "." +
ros_to_gams_name (topic);
gams::utility::ros::ros_to_gams_name (topic);
}
parser.parse_message (m, container_name);
@@ -438,24 +438,4 @@ std::string get_agent_var_prefix (std::string ros_topic_name)
}
}
std::string ros_to_gams_name (std::string ros_topic_name)
{
// Convert ros_topic_name to lower case
std::transform (ros_topic_name.begin (),
ros_topic_name.end (), ros_topic_name.begin (), ::tolower);
std::string name = ros_topic_name.substr (1);
std::string rosbag_robot_prefix = "robot_";
std::string topic = ros_topic_name;
if (name.find (rosbag_robot_prefix) == 0)
{
//remove the robot prefix
int namespace_end = name.find ("/") + 1;
//cut the prefix
topic = name.substr (namespace_end);
}
std::replace (topic.begin (), topic.end (), '/', '.');
return topic;
}
@@ -44,6 +44,8 @@ gams::transports::RosBridge::RosBridge (
gams::transports::RosBridge::~RosBridge ()
{
std::cout << "TERMINATE!!!" << std::endl << std::endl << std::endl << std::endl;
read_threads_.terminate();
}
long
@@ -58,6 +60,14 @@ gams::transports::RosBridge::send_data (
/**
* This is where you should do your custom transport sending logic/actions
**/
std::cout << std::endl << "SENDING UPDATE TO ROS!!!" << std::endl;
for (
madara::knowledge::VariableReferenceMap::const_iterator it = modifieds.begin();
it != modifieds.end(); it++)
{
const char * key = it->first;
std::cout << "Got update for " << key << std::endl;
}
std::cout << std::endl;
return result;
}
@@ -4,6 +4,16 @@
namespace knowledge = madara::knowledge;
void gams::transports::RosBridgeReadThread::messageCallback (
const topic_tools::ShapeShifter::ConstPtr& msg,
const std::string &topic_name )
{
std::cout << "CALLBACK FROM " << topic_name << " <" << msg->getDataType() << ">" << std::endl;
std::string container = gams::utility::ros::ros_to_gams_name(topic_name);
parser_->parse_message(msg, container);
}
// constructor
gams::transports::RosBridgeReadThread::RosBridgeReadThread (
const std::string & id,
@@ -20,6 +30,8 @@ gams::transports::RosBridgeReadThread::RosBridgeReadThread (
// destructor
gams::transports::RosBridgeReadThread::~RosBridgeReadThread ()
{
//sub_.shutdown();
delete[] parser_;
}
/**
@@ -28,12 +40,32 @@ gams::transports::RosBridgeReadThread::~RosBridgeReadThread ()
void
gams::transports::RosBridgeReadThread::init (knowledge::KnowledgeBase & knowledge)
{
std::cout << "Initializing RosBridgeReadThread\n" << std::endl;
// grab the context so we have access to update_from_external
context_ = &(knowledge.get_context ());
// setup the receive buffer
if (settings_.queue_length > 0)
buffer_ = new char [settings_.queue_length];
parser_ = new gams::utility::ros::RosParser(&knowledge, "world", "frame1");
char **argv;
int argc = 0;
ros::init(argc, argv, "ros_bridge");
ros::NodeHandle node;
const char* topic_names[]= { "test_1", "odom", "/tf" };
for (const char* topic_name: topic_names )
{
boost::function<void (const topic_tools::ShapeShifter::ConstPtr&)> callback;
callback = boost::bind (
&gams::transports::RosBridgeReadThread::messageCallback, this,
_1, topic_name );
subscribers_.push_back (node.subscribe( topic_name, 10, callback));
}
}
/**
@@ -55,6 +87,10 @@ gams::transports::RosBridgeReadThread::run (void)
gams::loggers::LOG_MAJOR,
"%s::run: executing\n", print_prefix);
ros::spinOnce();
ros::Duration(1).sleep();
/**
* this should store the number of bytes read into the buffer after your
* network, socket, serial port or shared memory read
@@ -87,4 +123,5 @@ gams::transports::RosBridgeReadThread::run (void)
* The next run of this method will be determined by read_thread_hertz
* in the QoSTransportSettings class that is passed in.
**/
//context_->print(0);
}
@@ -5,7 +5,22 @@
#include <string>
#include "madara/threads/BaseThread.h"
#include "gams/utility/ros/RosParser.h"
// ROS includes
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#pragma GCC diagnostic ignored "-Wreorder"
#endif
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "topic_tools/shape_shifter.h"
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
namespace gams
{
@@ -43,6 +58,9 @@ namespace gams
**/
virtual void run (void);
void messageCallback(const topic_tools::ShapeShifter::ConstPtr& msg,
const std::string &topic_name );
private:
/// data plane if we want to access the knowledge base
madara::knowledge::ThreadSafeContext * context_;
@@ -54,7 +72,7 @@ namespace gams
madara::transport::QoSTransportSettings settings_;
/// buffer for sending
madara::utility::ScopedArray <char> buffer_;
madara::utility::ScopedArray <char> buffer_;
/// data received rules, defined in Transport settings
madara::knowledge::CompiledExpression on_data_received_;
@@ -67,6 +85,11 @@ namespace gams
/// a specialty packet scheduler for experimental drop policies
madara::transport::PacketScheduler & packet_scheduler_;
std::vector<ros::Subscriber> subscribers_;
gams::utility::ros::RosParser * parser_;
};
}
} // end namespace threads
@@ -9,7 +9,8 @@
namespace global_ros = ros;
gams::utility::ros::RosParser::RosParser (knowledge::KnowledgeBase * kb,
std::string world_frame, std::string base_frame)
std::string world_frame, std::string base_frame) :
eval_settings_(true, true, false)
{
world_frame_ = world_frame;
base_frame_ = base_frame;
@@ -84,6 +85,67 @@ void gams::utility::ros::RosParser::parse_message (
}
void gams::utility::ros::RosParser::parse_message (
const topic_tools::ShapeShifter::ConstPtr& m,
std::string container_name)
{
if (m->getDataType () == "std_msgs/Int32")
{
int value = m->instantiate<std_msgs::Int32>()->data;
knowledge_->set (container_name, value, eval_settings_);
}
else if (m->getDataType () == "nav_msgs/Odometry")
{
parse_odometry (m->instantiate<nav_msgs::Odometry> ().get (),
container_name);
}
else if (m->getDataType () == "sensor_msgs/Imu")
{
parse_imu (m->instantiate<sensor_msgs::Imu> ().get (), container_name);
}
else if (m->getDataType () == "sensor_msgs/LaserScan")
{
parse_laserscan (m->instantiate<sensor_msgs::LaserScan> ().get (),
container_name);
}
else if (m->getDataType () == "geometry_msgs/Pose")
{
parse_pose (m->instantiate<geometry_msgs::Pose> ().get (),
container_name);
}
else if (m->getDataType () == "geometry_msgs/PoseStamped")
{
parse_pose (&m->instantiate<geometry_msgs::PoseStamped> ().get ()->pose,
container_name);
}
else if (m->getDataType () == "sensor_msgs/CompressedImage")
{
parse_compressed_image (
m->instantiate<sensor_msgs::CompressedImage> ().get (),
container_name);
}
else if (m->getDataType () == "sensor_msgs/PointCloud2")
{
parse_pointcloud2 (m->instantiate<sensor_msgs::PointCloud2> ().get (),
container_name);
}
else if (m->getDataType () == "sensor_msgs/Range")
{
parse_range (m->instantiate<sensor_msgs::Range> ().get (),
container_name);
}
else if (m->getDataType () == "sensor_msgs/FluidPressure")
{
parse_fluidpressure (m->instantiate<sensor_msgs::FluidPressure> ().get (),
container_name);
}
else if (m->getDataType () == "tf2_msgs/TFMessage")
{
parse_tf_message (m->instantiate<tf2_msgs::TFMessage> ().get ());
}
}
/**
* Parses unknown messages using ros_type_introspection
* DO NOT USE THIS FOR TYPES WITH LARGE ARRAYS
@@ -392,12 +454,16 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf)
*knowledge_, 3);
containers::NativeDoubleVector orientation ("agents.0.orientation",
*knowledge_, 3);
location.set (0, base_pose.as_location_vec ().get (0));
location.set (1, base_pose.as_location_vec ().get (1));
location.set (2, base_pose.as_location_vec ().get (2));
orientation.set (0, base_pose.as_orientation_vec ().get (0));
orientation.set (1, base_pose.as_orientation_vec ().get (1));
orientation.set (2, base_pose.as_orientation_vec ().get (2));
location.set (0, base_pose.as_location_vec ().get (0), eval_settings_);
location.set (1, base_pose.as_location_vec ().get (1), eval_settings_);
location.set (2, base_pose.as_location_vec ().get (2), eval_settings_);
orientation.set (0, base_pose.as_orientation_vec ().get (0),
eval_settings_);
orientation.set (1, base_pose.as_orientation_vec ().get (1),
eval_settings_);
orientation.set (2, base_pose.as_orientation_vec ().get (2),
eval_settings_);
}
catch ( gams::pose::unrelated_frames ex){}
}
@@ -482,7 +548,8 @@ void gams::utility::ros::RosParser::parse_twist (geometry_msgs::Twist *twist,
void gams::utility::ros::RosParser::parse_pose (geometry_msgs::Pose *pose,
std::string container_name)
{
containers::NativeDoubleVector cont (container_name, *knowledge_, 6);
containers::NativeDoubleVector cont (container_name, *knowledge_, 6,
eval_settings_);
gams::pose::Quaternion quat (pose->orientation.x,
pose->orientation.y,
pose->orientation.z,
@@ -584,3 +651,25 @@ void gams::utility::ros::RosParser::parse_int_array (boost::array<int, N> *array
i++;
}
}
std::string gams::utility::ros::ros_to_gams_name (std::string ros_topic_name)
{
// Convert ros_topic_name to lower case
std::transform (ros_topic_name.begin (),
ros_topic_name.end (), ros_topic_name.begin (), ::tolower);
std::string name = ros_topic_name.substr (1);
std::string rosbag_robot_prefix = "robot_";
std::string topic = ros_topic_name;
if (name.find (rosbag_robot_prefix) == 0)
{
//remove the robot prefix
int namespace_end = name.find ("/") + 1;
//cut the prefix
topic = name.substr (namespace_end);
}
std::replace (topic.begin (), topic.end (), '/', '.');
return topic;
}
Oops, something went wrong.

0 comments on commit d74cc85

Please sign in to comment.