路径就是画不出来,求助
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就是画不出来,求问错在哪里?
{
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就是画不出来,求问错在哪里?