Line Strips and Points in Marker array not being published with correct pose

edit

Hi,

I'm trying to publish a marker for every obstacle I encounter in my path. If I specify the type of marker as cylinder or cube or sphere the marker is published in the correct pose that I specify. I wanted to extend this by using points and line strips to create a polygon type boundary. In order to create the points from pose I use the "costmap_2d::transformFootprint" and obtain my points.

The problem is the unlike the cylinder object the line strips seem to have a offset. This is really weird as when I check the topic in the terminal my points used to construct the polygon seem to be right.

This is the code:

                double x = this_object.position.x,
                y = this_object.position.y,
                yaw = this_object.yaw;

                dynamic_object_msg_.pose.position.x = this_object.position.x;
                dynamic_object_msg_.pose.position.y = this_object.position.y;
                dynamic_object_msg_.pose.position.z = 0;
                dynamic_object_msg_.header.stamp = ros::Time::now();
                dynamic_object_msg_.header.frame_id = "map";    
                dynamic_object_msg_.id = i;
                dynamic_object_msg_.type = visualization_msgs::Marker::CYLINDER;
                dynamic_object_msg_.action = visualization_msgs::Marker::ADD;
                dynamic_object_msg_.scale.x = 1;
                dynamic_object_msg_.scale.y = 1;
                dynamic_object_msg_.scale.z = 1;


                line_strip.type = visualization_msgs::Marker::LINE_STRIP;
                line_strip.action = visualization_msgs::Marker::ADD;
                line_strip.pose.position.x = this_object.position.x;
                line_strip.pose.position.y = this_object.position.y;
                line_strip.pose.position.z = 0;
                line_strip.header.stamp = ros::Time::now();
                line_strip.header.frame_id = "map"; 
                line_strip.scale.x = 0.4;


                dynamic_object_msg_.color.a = 1.0; 
                dynamic_object_msg_.color.r = 1.0;
                dynamic_object_msg_.color.g = 0.0;
                dynamic_object_msg_.color.b = 0.0;   

                line_strip.color.g = 1.0;
                line_strip.color.a = 1.0;

            point.x = this_object.length;
            point.y = this_object.width;
            point.z = 0;
            padded_object_footprint_.push_back(point);

            point.x = this_object.length + 1;
            point.y = 0;
            point.z = 0;        
            padded_object_footprint_.push_back(point);

            point.x = this_object.length;
            point.y = -1*this_object.width;
            point.z = 0;
            padded_object_footprint_.push_back(point);

            point.x = -1*this_object.length;
            point.y = -1*this_object.width;
            point.z = 0;
            padded_object_footprint_.push_back(point);

            point.x = -1*this_object.length;
            point.y = this_object.width;
            point.z = 0;
            padded_object_footprint_.push_back(point);

            point.x = this_object.length;
            point.y = this_object.width;
            point.z = 0;
            padded_object_footprint_.push_back(point);
            costmap_2d::transformFootprint(x, y, yaw, padded_object_footprint_, oriented_footprint);

            c.r = 0.0;
            c.g = 1.0;
            c.b = 0.0;
            c.a = 1.0;

            for(auto &f: oriented_footprint){
                dynamic_object_msg_.points.push_back(f);
                line_strip.points.push_back(f);
                dynamic_object_msg_.colors.push_back(c);
            }
            objects_msg_.markers.push_back(dynamic_object_msg_);
            line_strip_msg_.markers.push_back(line_strip);
        }
            rviz_objects_pub_.publish(objects_msg_);
            rviz_objects_pub_.publish(line_strip_msg_);

This is the rostopic output:

markers: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1573656484
        nsecs:  58161258
      frame_id: "map"
    ns: ''
    id: 0
    type: 2
    action: 0
    pose: 
      position: 
        x: -171.17551556
        y: 0.383408119498
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0
    scale: 
      x: 1.0
      y: 1.0
      z: 1.0
    color: 
      r: 0.0
      g: 1.0
      b: 0.0
      a: 1.0
    lifetime: 
      secs: 0
      nsecs:         0
    frame_locked: True
    points: 
      - 
        x: -174.061346865
        y: -3.26388946415
        z: 0.0
      - 
        x: -175.810768995
        y: -2.14499608159
        z: 0.0
      - 
        x: -175.804413379
        y: -0 ...