0.前言
我在上一篇博文07轨迹规划仿真中实现PTP的轨迹规划，并且把圆弧轨迹规划的坑挖好了，但是我一直很难受的地方就是终端的轨迹显示，我之前解释了我实习UR10moveit控制的方式，是使用官方panda的教程进行修改，但是panda的教程是可以用绿色轨迹线显示轨迹的，这篇blog我想处理一下这个问题。
感谢博主的文章，激发我的灵感
https://blog.csdn.net/gophae/article/details/108514336
1.解读参考博客的代码
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <cstdlib>
main(int argc, char **argv)
{
ros::init(argc, argv, "showpath");
ros::NodeHandle ph;
ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory", 1, true);//关键代码1 出现轨迹
ros::Publisher point_pub = ph.advertise<geometry_msgs::PointStamped>("point", 1, true);//关键代码2 出现point点
//这三行貌似是处理时间问题，我觉得他和我的轨迹轨迹时间戳需要共用
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
nav_msgs::Path path;//定义path
//nav_msgs::Path path
path.header.stamp = current_time;
path.header.frame_id = "odom";
double x = 0.0;
double y = 0.0;
double z = 0.0;
double th = 0.0;
double vx = 0.1 + 0.2 * rand() / double(RAND_MAX);
double vy = -0.1 + 0.2 * rand() / double(RAND_MAX);
double vth = 0.1;
ros::Rate loop_rate(10);
while (ros::ok())
{
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
double dt = 1;
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
z += 0.005;
th += delta_th;
geometry_msgs::PoseStamped this_pose_stamped;
geometry_msgs::PointStamped this_point_stamped;
this_pose_stamped.pose.position.x = x;
this_pose_stamped.pose.position.y = y;
this_pose_stamped.pose.position.z = z;
this_point_stamped.header.stamp = current_time;
this_point_stamped.header.frame_id = "odom";
this_point_stamped.point.x = x;
this_point_stamped.point.y = y;
this_point_stamped.point.z = z;
ROS_INFO("current_x: %f", x);
ROS_INFO("current_y: %f", y);
geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
this_pose_stamped.pose.orientation.x = goal_quat.x;
this_pose_stamped.pose.orientation.y = goal_quat.y;
this_pose_stamped.pose.orientation.z = goal_quat.z;
this_pose_stamped.pose.orientation.w = goal_quat.w;
this_pose_stamped.header.stamp = current_time;
this_pose_stamped.header.frame_id = "odom";
path.poses.push_back(this_pose_stamped);//关键代码
path_pub.publish(path);//关键代码
point_pub.publish(this_point_stamped);
ros::spinOnce(); // check for incoming messages
last_time = current_time;
loop_rate.sleep();
}
return 0;
}
2.设置rviz界面
实时显示轨迹的中心思想:就是在代码中publish一个/trajectory 的topic ，然后在rviz中将其调用显示
设置rviz的界面流程参考：//一定要去看
https://blog.csdn.net/gophae/article/details/108514336
和上文是同一个博客
tip：这个位置我打开了rviz面板功能的新世界的大门！我之前想看joint数组要去终端用rostopic 我真是～笨死了！
3.更改UR10轨迹规划的cpp文件
插曲：我思考了一下，更改cpp的目的，就是想要一个topic,包含末端位置和时间戳，我觉得moveit肯定发布了这个话题，只不过我没有合理使用他，所以我打算试一下
cd ~/ws_moveit
source ./devel/setup.bash
roslaunch ur10_moveit_config demo.launch
新的tab
cd ~/ws_moveit
source ./devel/setup.bash
roslaunch moveit_tutorials move_group_interface_tutorial.launch
新的tab
rqt_graph
然后查看这些话题谁最“可疑”
我最后选定了
rostopic echo /place/status
运行结果：
header:
seq: 2068
stamp:
secs: 1613531538
nsecs: 409575573
frame_id: ‘’
status_list: []
我发现只有时间戳，没有位置信息
对比一下参考博客中的/trajectory
header:
seq: 0
stamp:
secs: 1613533245
nsecs: 30911903
frame_id: “odom”
pose:
position:
x: -1.75693075225
y: 4.34585964254
z: 1.465
orientation:
x: -0.0
y: 0.0
z: 0.87135797221
w: -0.490647821015
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <cstdlib>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_ur10_circle");
ros::NodeHandle node_handle;
ros::Publisher path_pub = node_handle.advertise<nav_msgs::Path>("trajectory", 1, true);//path
ros::Time current_time, last_time;//path
current_time = ros::Time::now();//path
last_time = ros::Time::now();//path
nav_msgs::Path path;//path
//nav_msgs::Path path;
path.header.stamp = current_time;//path
path.header.frame_id = "base_link";//path
double x = 0.0;//path
int i = 0;//path
double y = 0.0;//path
double z = 0.0;//path
double th = 0.0;//path
double vx = 0.1 + 0.2 * rand() / double(RAND_MAX);//path
double vy = -0.1 + 0.2 * rand() / double(RAND_MAX);//path
double vth = 0.1;//path
ros::AsyncSpinner spinner(1);
spinner.start();
///将“manipulator”储存在joint_model_group中
static const std::string PLANNING_GROUP = "manipulator";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization
// ^^^^^^^^^^^^^
//可视化部分包括对象、机器人、轨迹的显示
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();
//远程控制是一种自省工具，允许用户通过RViz中的按钮和键盘快捷键逐步完成高级脚本
visual_tools.loadRemoteControl();
//RViz提供了许多类型的标记，在这个演示中，我们将使用文本、圆柱体和球体
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
// 批量发布用于减少发送到RViz的大型可视化消息的数量
visual_tools.trigger();
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//得到基本信息
// 批量发布用于减少发送到RViz的大型可视化消息的数量
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
// 我们还可以打印此组的末端效应器链接的名称
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
//我们可以得到机器人中所有组的列表：
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// Start the demo
// ^^^^^^^^^^^^^^^^^^^^^^^^^
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
//把当前状态设置为运动的初始状态
current_time = ros::Time::now();//path
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
robot_state::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.52;
start_pose2.position.y = -0.02;
start_pose2.position.z = 0.2;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);
std::vector<geometry_msgs::Pose> waypoints;
//waypoints.push_back(start_pose2);
geometry_msgs::Pose target_pose3 = start_pose2;
geometry_msgs::PoseStamped this_pose_stamped;//path
while (i<70)
{
i++;
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
double dt = 1;
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
z += 0.005;
th += delta_th;
geometry_msgs::PoseStamped this_pose_stamped;
geometry_msgs::PointStamped this_point_stamped;
this_pose_stamped.pose.position.x = x*0.1+0.5;
this_pose_stamped.pose.position.y = y*0.1;
this_pose_stamped.pose.position.z = z*0.1+0.2;
target_pose3.position.x = this_pose_stamped.pose.position.x;
target_pose3.position.y = this_pose_stamped.pose.position.y;
target_pose3.position.z = this_pose_stamped.pose.position.z;
waypoints.push_back(target_pose3);// 1
geometry_msgs::Quaternion goal_quat =
tf::createQuaternionMsgFromYaw(th);
this_pose_stamped.pose.orientation.x = goal_quat.x;
this_pose_stamped.pose.orientation.y = goal_quat.y;
this_pose_stamped.pose.orientation.z = goal_quat.z;
this_pose_stamped.pose.orientation.w = goal_quat.w;
this_pose_stamped.header.stamp = current_time;
this_pose_stamped.header.frame_id = "base_link";
path.poses.push_back(this_pose_stamped);
path_pub.publish(path);
ros::spinOnce(); // check for incoming messages
last_time = current_time;
//loop_rate.sleep();
}
move_group.setMaxVelocityScalingFactor(0.1);
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// We can also visualize the plan as a line with markers in RViz.
ros::shutdown();
return 0;
}
简单说一下这个代码，在我上一篇博客中也写到，其实就是实现圆弧轨迹规划，然后定义圆弧差值，之后把每个插值点同时做两件事：
①赋值给path点，然后发布到/trajectory上
②赋值target_pose3 进行轨迹规划
这样随着机械臂实现这个圆弧轨迹，也会看到/trajectory话题的轨迹曲线
最后我发现这个代码有点问题，就是显示的轨迹是我们的目标轨迹，假如实际情况只可以实现80%，绿色轨迹还是会出现100%，我觉得不太对头，后续改进～
评论（0）
您还未登录，请登录后发表或查看评论