路径就是画不出来，求助- ROS-ROS机器人开发社区

路径就是画不出来，求助

热门

piliboss 原型机 9079 5

2016-01-17 03:47

热门 2016-01-17 03:47 9079 5

int main( int argc, char** argv )
{
  ros::init(argc, argv, "robot_control2");
  ros::NodeHandle n("~");

Eigen::Vector4d Points[9];
double pitch=0.187;

Points[0] << 0.266, 0.000, -0.001, pitch;
Points[1] << 0.266, 0.050, -0.001,pitch;
Points[2] << 0.316, 0.000, -0.001, pitch;
Points[3] << 0.316, 0.050, -0.001, pitch;
Points[4] = Points[1];
Points[5] << 0.291, 0.075, -0.001,pitch;
Points[6] = Points[3];
Points[7] = Points[0];
Points[8] = Points[2];

ros::Publisher PublishDesired = n.advertise<nav_msgs::Path>("/phantomx/desired_path",1000);
nav_msgs::Path des_path;
des_path.header.stamp = ros::Time::now();
des_path.header.frame_id = "/arm_base_link";

for(int i = 0; i<9; ++i)
{ geometry_msgs::PoseStamped pos_es;
   pos_es.header.stamp = ros::Time::now();
   pos_es. header.frame_id = "/arm_base_link";
   pos_es.pose.position.x = Points【i】(0);
   pos_es.pose.position.y = Points【i】(1);
   pos_es.pose.position.z = Points【i】(2);
   pos_es.pose.orientation.w = 1;
   des_path.poses.push_back(pos_es);
}
   PublishDesired.publish(des_path);
   return 0;
}

路径在rviz就是画不出来，求问错在哪里？