0
$\begingroup$

Rosanswers logo

Hi everyone,

I am trying to subscribe to a message of type nav_msgs::Path. Following is the callback function:

    nav_msgs::Path gPlan;
    void planCallback(nav_msgs::Path::ConstPtr& msg)
    {
         int i=0;
          for(std::vector<geometry_msgs::PoseStamped>::const_iterator it= msg->poses.begin(); it!= msg->poses.end();  ++it)
          {
    
              gPlan.poses[i] = *it;
              i++;
           }
      }

The subscriber has been defined as:

ros::Subscriber planSub = n.subscribe("global_plan", 1000, planCallback);

The error I am getting is as follows:

In file included from /opt/ros/indigo/include/ros/serialization.h:37:0,
                 from /opt/ros/indigo/include/ros/publisher.h:34,
                 from /opt/ros/indigo/include/ros/node_handle.h:32,
                 from /opt/ros/indigo/include/ros/ros.h:45,
                 from /home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:1:
/opt/ros/indigo/include/ros/message_traits.h: In instantiation of ‘static const char* ros::message_traits::MD5Sum<M>::value() [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’:
/opt/ros/indigo/include/ros/message_traits.h:228:103:   required from ‘const char* ros::message_traits::md5sum() [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’
/opt/ros/indigo/include/ros/subscribe_options.h:89:50:   required from ‘void ros::SubscribeOptions::initByFullCallbackType(const string&, uint32_t, const boost::function<void(P)>&, const boost::function<boost::shared_ptr<typename ros::ParameterAdapter<P>::Message>()>&) [with P = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; std::string = std::basic_string<char>; uint32_t = unsigned int; typename ros::ParameterAdapter<P>::Message = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’
/opt/ros/indigo/include/ros/node_handle.h:658:5:   required from ‘ros::Subscriber ros::NodeHandle::subscribe(const string&, uint32_t, void (*)(M), const ros::TransportHints&) [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; std::string = std::basic_string<char>; uint32_t = unsigned int]’
/home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:71:74:   required from here
/opt/ros/indigo/include/ros/message_traits.h:121:29: error: ‘__s_getMD5Sum’ is not a member of ‘boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >’
     return M::__s_getMD5Sum().c_str();
                             ^
/opt/ros/indigo/include/ros/message_traits.h: In instantiation of ‘static const char* ros::message_traits::DataType<M>::value() [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’:
/opt/ros/indigo/include/ros/message_traits.h:237:105:   required from ‘const char* ros::message_traits::datatype() [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’
/opt/ros/indigo/include/ros/subscribe_options.h:90:54:   required from ‘void ros::SubscribeOptions::initByFullCallbackType(const string&, uint32_t, const boost::function<void(P)>&, const boost::function<boost::shared_ptr<typename ros::ParameterAdapter<P>::Message>()>&) [with P = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; std::string = std::basic_string<char>; uint32_t = unsigned int; typename ros::ParameterAdapter<P>::Message = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’
/opt/ros/indigo/include/ros/node_handle.h:658:5:   required from ‘ros::Subscriber ros::NodeHandle::subscribe(const string&, uint32_t, void (*)(M), const ros::TransportHints&) [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; std::string = std::basic_string<char>; uint32_t = unsigned int]’
/home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:71:74:   required from here
/opt/ros/indigo/include/ros/message_traits.h:138:31: error: ‘__s_getDataType’ is not a member of ‘boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >’
     return M::__s_getDataType().c_str();
                               ^
In file included from /opt/ros/indigo/include/ros/subscription_callback_helper.h:35:0,
                 from /opt/ros/indigo/include/ros/subscriber.h:33,
                 from /opt/ros/indigo/include/ros/node_handle.h:33,
                 from /opt/ros/indigo/include/ros/ros.h:45,
                 from /home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:1:
/opt/ros/indigo/include/ros/parameter_adapter.h: In instantiation of ‘static ros::ParameterAdapter<M>::Parameter ros::ParameterAdapter<M>::getParameter(const Event&) [with M = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; ros::ParameterAdapter<M>::Parameter = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; ros::ParameterAdapter<M>::Event = ros::MessageEvent<const boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > > >; typename boost::remove_reference<typename boost::remove_const<M>::type>::type = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >]’:
/opt/ros/indigo/include/ros/subscription_callback_helper.h:144:54:   required from ‘void ros::SubscriptionCallbackHelperT<P, Enabled>::call(ros::SubscriptionCallbackHelperCallParams&) [with P = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; Enabled = void]’
/home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:127:1:   required from here
/opt/ros/indigo/include/ros/parameter_adapter.h:78:30: error: invalid initialization of reference of type ‘ros::ParameterAdapter<boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&>::Parameter {aka boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&}’ from expression of type ‘const boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >’
     return *event.getMessage();
                              ^
In file included from /opt/ros/indigo/include/ros/publisher.h:34:0,
                 from /opt/ros/indigo/include/ros/node_handle.h:32,
                 from /opt/ros/indigo/include/ros/ros.h:45,
                 from /home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:1:
/opt/ros/indigo/include/ros/serialization.h: In instantiation of ‘static void ros::serialization::Serializer<T>::read(Stream&, typename boost::call_traits<T>::reference) [with Stream = ros::serialization::IStream; T = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >; typename boost::call_traits<T>::reference = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&]’:
/opt/ros/indigo/include/ros/serialization.h:163:32:   required from ‘void ros::serialization::deserialize(Stream&, T&) [with T = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >; Stream = ros::serialization::IStream]’
/opt/ros/indigo/include/ros/subscription_callback_helper.h:136:34:   required from ‘ros::VoidConstPtr ros::SubscriptionCallbackHelperT<P, Enabled>::deserialize(const ros::SubscriptionCallbackHelperDeserializeParams&) [with P = boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >&; Enabled = void; ros::VoidConstPtr = boost::shared_ptr<const void>]’
/home/lead/catkin_ws_autoware/src/PPpublisher/src/purepursuitPub.cpp:127:1:   required from here
/opt/ros/indigo/include/ros/serialization.h:136:5: error: ‘class boost::shared_ptr<const nav_msgs::Path_<std::allocator<void> > >’ has no member named ‘deserialize’
     t.deserialize(stream.getData());
     ^
make[2]: *** [PPpublisher/CMakeFiles/purepursuitPub.dir/src/purepursuitPub.cpp.o] Error 1
make[1]: *** [PPpublisher/CMakeFiles/purepursuitPub.dir/all] Error 2
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

Where am I going wrong?

edit:

The whole code is:

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <autoware_msgs/lane.h>
#include <tf2_msgs/TFMessage.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <iostream>
#include <cstring> 
#include <std_msgs/Float32.h>

using namespace std;

tf2_msgs::TFMessage tfmsg;
nav_msgs::Path gPlan;


float curVel;

void tfCallback(const tf2_msgs::TFMessage& msg)
{
    tfmsg = msg;
}
void planCallback(nav_msgs::Path::ConstPtr& msg)
{
  int i=0;
  for(std::vector<geometry_msgs::PoseStamped>::const_iterator it= msg->poses.begin(); it!= msg->poses.end(); ++it)
  {
   
    gPlan.poses[i] = *it;
    i++;
  }
}

void encCallback(const std_msgs::Float32::ConstPtr& enc_msg)
{
  curVel = enc_msg->data;
}

int main(int argc, char **argv)
{
  
  ros::init(argc, argv, "pp_listener");

 
  ros::NodeHandle n;

  
  geometry_msgs::PoseStamped curPose;
  geometry_msgs::PoseStamped curPoseTransformed;
  autoware_msgs::lane finWP;
  autoware_msgs::waypoint wp;



   tf::TransformListener listener;


  ros::Subscriber tfSub = n.subscribe("tf", 1000, tfCallback);
  ros::Subscriber planSub = n.subscribe("global_plan", 1000, planCallback);
  ros::Subscriber encSub = n.subscribe("enc_vel", 1000, encCallback);


  ros::Publisher cur_posePub = n.advertise<geometry_msgs::Pose>("current_pose", 1000);
  ros::Publisher cur_velPub = n.advertise<std_msgs::Float32>("current_velocity", 1000);
  ros::Publisher fin_wpPub = n.advertise<autoware_msgs::lane>("final_Waypoints", 1000);
  ros::Rate r(10.0);
  while(ros::ok())
  {
    ros::spinOnce();

    ros::Time now = ros::Time::now();

    //curVel = odommsg.twist.twist.linear.x;




    if(tfmsg.transforms[2].child_frame_id == "base_link")
    {
      curPose.pose.position.x  = tfmsg.transforms[2].transform.translation.x;
      curPose.pose.position.y  = tfmsg.transforms[2].transform.translation.y;
      curPose.pose.position.z  = tfmsg.transforms[2].transform.translation.z;
      curPose.pose.orientation = tfmsg.transforms[2].transform.rotation;
      curPose.header = tfmsg.transforms[2].header;
      listener.transformPose("map",curPose,curPoseTransformed);
    }

    finWP.header = gPlan.header;
    finWP.increment =1;

    
    for(int i=0; i<=gPlan.poses.size(); i++)
    {
      wp.pose.header = gPlan.header;
      wp.twist.header = gPlan.header;
      wp.twist.twist.linear.x = 2;
      wp.pose.pose.position = gPlan.poses[i].pose.position;
      wp.pose.pose.orientation = gPlan.poses[i].pose.orientation;
      finWP.waypoints.push_back(wp);
    }


     cur_posePub.publish(curPose);
     cur_velPub.publish(curVel);
     fin_wpPub.publish(finWP);

    r.sleep();
  }
  
  return 0;
}

Originally posted by kllysin on ROS Answers with karma: 13 on 2018-11-19

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

If you just want to create a copy of the path message, then the copy constructors mean you can do this:

gPlan = nav_msgs::Path(msg);

Hope this helps.


Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-11-19

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by kllysin on 2018-11-19:
Thanks for your answer!

Comment by MichelleHusbands on 2020-11-12:
Thanks, @PeteBlackerThe3rd, it worked like a charm!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.