Robotics StackExchange | Archived questions

Can ecto (ecto_ros) publish custom ros msgs?

I'm working with ecto and ROS and would like to publish custom msgs from ecto. Is it possible? If yes, what would be the best way to do so?
For example, that kind of message:
rosmsg show mycustommsgs/custommsg
--std
msgs/Header header
----uint32 seq
----time stamp
--string frameid
string name
geometry
msgs/Pose pose
--geometry_msgs/Point position
----float64 x
----float64 y
----float64 z

Asked by stanislas.brossette on 2013-12-12 05:43:22 UTC

Comments

I am also trying to publish TF messages from Ecto. If I include ros headers I get a library conflict and my program doesn't work anymore. Did you find a way to do that?

Asked by Mehdi. on 2014-11-02 22:32:46 UTC

Hi Medhi, Sadly no, we haven't found a way to do that. Though, including ros headers hasn't been a problem. I haven't worked on that for a very long time so... But if you want you can take a look at my repo about that: git@github.com:stanislas-brossette/cloud-treatment-ecto.git

Asked by stanislas.brossette on 2014-11-03 19:29:14 UTC

Answers

Not fully sure if this is is what you need, but Daniel Stonier sent me this example of an ecto cell with internal ros publisher :

/**
 * @file /dslam_ecto_bridge/src/dslam_viz/rviz_relay.cpp
 * 
 * @brief Short description of this file.
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <dslam_central/statistics.hpp>
#include <dslam_msgs/Map.h>
#include <ecto/ecto.hpp>
#include <iostream>
#include <memory>
#include <ros/ros.h>
#include <string>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace dslam_ecto_bridge {
namespace cells {

/*****************************************************************************
 ** Cell
 *****************************************************************************/

struct RVizRelay
{
  RVizRelay()
   : throttle_counter(1)
  {}

  static void declare_params(ecto::tendrils& params)
  {
    params.declare<unsigned int>(&RVizRelay::every_n_,
                         "every_n",
                         "Throttle publishing to only 1 in every n executions.",
                         1
                         );
  }

  static void
  declare_io(const ecto::tendrils& params, ecto::tendrils& in, ecto::tendrils& out)
  {
//    in.declare<dslam_viewer::StereoParameters>("stereo_parameters", "Structure containing all the stereo parameters.");
    in.declare<dslam_central::Statistics>(&RVizRelay::statistics_,  "statistics", "DamnSlam tracker-mapper statistics.");
  }

  void configure(const ecto::tendrils& params, const ecto::tendrils& in, const ecto::tendrils& out)
  {
    ros::NodeHandle nh("~");
    nh.param<std::string>("global_frame_id", global_frame_id, "dslam_map");
    dslam_keyframes_publisher = nh.advertise<dslam_msgs::Map>("keyframes", 5, true);
    //    stereo_parameters = in.get<dslam_viewer::StereoParameters>("stereo_parameters");
  }

  int process(const ecto::tendrils& in, const ecto::tendrils& out)
  {
    // we actually want to publish every time we get a frame update
//    int num_subscribers = dslam_keyframes_publisher.getNumSubscribers();
//    if ( num_subscribers != 0 && throttle_counter == *every_n_) {
      for ( const auto &keyframe_statistics : statistics_->keyframe_statistics_queue ) {
        dslam_msgs::Map msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = global_frame_id;
        for (const auto& id_keyframe_pair : keyframe_statistics->frames) {
          dslam_msgs::KeyFrame msg_keyframe;
          msg_keyframe.id = id_keyframe_pair.second.id;
          msg_keyframe.transform.translation.x = id_keyframe_pair.second.T_keyframe_rel_map.translation().x();
          msg_keyframe.transform.translation.y = id_keyframe_pair.second.T_keyframe_rel_map.translation().y();
          msg_keyframe.transform.translation.z = id_keyframe_pair.second.T_keyframe_rel_map.translation().z();
          msg_keyframe.transform.rotation.x = id_keyframe_pair.second.T_keyframe_rel_map.unit_quaternion().x();
          msg_keyframe.transform.rotation.y = id_keyframe_pair.second.T_keyframe_rel_map.unit_quaternion().y();
          msg_keyframe.transform.rotation.z = id_keyframe_pair.second.T_keyframe_rel_map.unit_quaternion().z();
          msg_keyframe.transform.rotation.w = id_keyframe_pair.second.T_keyframe_rel_map.unit_quaternion().w();
          msg.keyframes.push_back(msg_keyframe);
        }
        for (const auto& edge : keyframe_statistics->edges) {
          dslam_msgs::Edge msg_edge;
          msg_edge.begin = edge.first;
          msg_edge.end = edge.second;
          msg.edges.push_back(msg_edge);
        }
        dslam_keyframes_publisher.publish(msg);
      }
//    }
    throttle_counter = ( throttle_counter == *every_n_) ? 1 : throttle_counter + 1;
    return ecto::OK;
  }

private:
  // variables
  ros::Publisher dslam_keyframes_publisher;
  unsigned int throttle_counter;
  std::string global_frame_id;
  // param tendrils
  ecto::spore<unsigned int> every_n_;
  // input tendrils
  ecto::spore<dslam_central::Statistics> statistics_;
};

} // namespace cells
} // namespace dslam_ecto_bridge

// This macro is required to register the cell with the module
// first argument: the module that it is to be part of
// second argument: the cell we want to expose in the module
// third argument: the name of that cell as seen in Python
// fourth argument: a description of what that cell does
ECTO_CELL(dslam_viz, dslam_ecto_bridge::cells::RVizRelay, "RVizRelay", "Relays data to the dslam rviz plugin.");

Asked by jorge on 2014-11-05 08:08:52 UTC

Comments

Thanks for sharing this. It goes straight forward just include ros.h , in my case I had some problems because my TF broadcaster was declared as static.

Asked by Mehdi. on 2014-11-05 20:26:11 UTC