通过外部程序获得Moveit-Rviz的MotionPlanning面板相同的操控能力

2023-12-02 01:08
文章标签 相同 程序 外部 面板 能力 操控 获得 rviz moveit motionplanning

本文主要是介绍通过外部程序获得Moveit-Rviz的MotionPlanning面板相同的操控能力，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

在Moveit-Rviz启动之后，在机器人的末端会显示一个Interactive Marker用来指示目标方位，可以通过鼠标来移动和旋转该Marker，操作时会有一个虚拟的机器人随之运动。当按下MotionPlanning面板中的Plan按钮便会显示从当前方位到目标方位的一个运动轨迹。当按下Execute按钮后机器人就会实际运动到目标方位。

Moveit在rviz中提供的操控Topic

当勾选MotionPlanning面板中的"Allow External Comm."选项的时候，在ROS上就会出现如下的几个Topic:

  • /rviz/moveit/plan : 和Rviz中MotionPlanning面板提供的Plan按钮功能一样，该topic的消息类型为std_msgs/Empty，发送一个这样的消息到该Topic就相当与按下了Plan按钮。
  • /rviz/moveit/execute : 和Rviz中MotionPlanning面板提供的Execute按钮功能一样
  • /rviz/moveit/select_planning_group : 用于选择你想要控制的Planning Group的Topic。其消息类型为std_msgs/String。你可以在命令终端下选择Planning Group，例如
    rostopic pub /rviz/moveit/select_planning_group std_msgs/String "hand"
  • /rviz/moveit/update_{goal,start}_state : 和MotionPlanning面板中"State State"和"Goal State"下选框中的"current"是同样的功能。
  • /rviz/moveit/update_custom_{goal,start}_state : 该Topic用于给机器人设置一个指定的状态，其消息类型为moveit_msgs/RobotState。
  • /rviz/moveit/move_marker/goal_#{link_name} : 指定交互式marker的方位的Topic。link_name是Planning Group中的末端连杆的名称，例如"/rviz/moveit/move_marker/goal_panda_link8。其中"panda_link8"为panda机器人本体的最后一个连杆。该Topic的消息类型为geometry_msgs/PoseStamped。

另外，Moveit发布如下的Topic

  • /rviz_moveit_motion_planning_display/robot_interactive_interactive_marker_topic/update : 该Topic会发布经过确认的有效的IMarker方位。我们可以用来更新IMarker的实际goalPose。
  • /execute_trajectory/status : 当moveit通过Execute执行实际的轨迹运动时，就会通过该Topic发布执行的状态。因此当我们进行实际的轨迹运动时就可以通过该Topic来检查完成结果。该Topic类型为actionlib_msgs/GoalStatusArray。

使用键盘的简单控制

有了上面的Topic，我们便可以通过编写程序来操控rviz中的机器人，最主要的就是可以自己编写程序控制机器人上的Interactive Marker，例如使用键盘和手柄控制，而非只是简单地在屏幕上进行非精确地拖动。

在这个例子中，我们使用adsw按键来控制Marker在XY平面的移动，通过上下箭头按键控制X方向上的移动。

#!/usr/bin/env python
# coding: UTF-8import rospy
import sys, select, termios, ttyfrom moveit_msgs.msg import RobotState
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import InteractiveMarkerUpdate
import moveit_commanderclass IMarkerKeyboard:def __init__(self):self.pre_pose= PoseStamped()self.planning_groups_tips = {}self.ttySettings=termios.tcgetattr(sys.stdin)# 获得机器人的当前实际位置group_name = "arm"  # 这是对机器人进行moveit配置定义的关节组名称self.group = moveit_commander.MoveGroupCommander(group_name)self.robot = moveit_commander.RobotCommander()self.curPose=self.group.get_current_pose()self.initState= self.robot.get_current_state()# 创建相关self.goalState_pub = rospy.Publisher("/rviz/moveit/update_custom_goal_state",RobotState,queue_size=5)self.marker_pub = rospy.Publisher("/rviz/moveit/move_marker/goal_link_6",PoseStamped,queue_size=5)self.update_sub = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update",InteractiveMarkerUpdate,self.updateGoal)rospy.sleep(2)  # 确保rviz能收到消息# 将IMarker移动到与实际位置一致self.goalState_pub.publish(self.initState)rospy.sleep(1)# 回调函数通过接收update Topic来更新Marker的目标位置def updateGoal(self, msg):if len(msg.poses) > 0:self.curPose.pose.position = msg.poses[0].pose.positionself.curPose.pose.orientation = msg.poses[0].pose.orientationdef getKey(self):tty.setraw(sys.stdin.fileno())select.select([sys.stdin],[],[],0)key= sys.stdin.read(1)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.ttySettings)return keydef run(self):print("wait key...")while(True) :key = self.getKey()if key=='-':goalPose=self.curPosegoalPose.pose.position.z -=0.01self.marker_pub.publish(goalPose)elif key=='=':goalPose=self.curPosegoalPose.pose.position.z +=0.01self.marker_pub.publish(goalPose)elif key=='a':goalPose=self.curPosegoalPose.pose.position.x -=0.01self.marker_pub.publish(goalPose)elif key == 'd':goalPose =self.curPosegoalPose.pose.position.x += 0.01self.marker_pub.publish(goalPose)elif key == 's':goalPose =self.curPosegoalPose.pose.position.y -= 0.01self.marker_pub.publish(goalPose)elif key == 'w':goalPose =self.curPosegoalPose.pose.position.y += 0.01self.marker_pub.publish(goalPose)if(key == '\x03'):  # Ctrl+Cbreakdef main():rospy.init_node('marker_test', anonymous=True)app=IMarkerKeyboard()app.run()returnif __name__ == '__main__':main()

使用手柄的简单控制

在手柄控制中，使用如下按键或摇杆进行控制：

  • 左摇杆上下： X方向粗控制
  • 左摇杆左右： Y方向粗控制
  • LEFT-RIGHT: X方向细控制
  • UP-DOWN: Y方向细控制
  • 右摇杆上下： Z方向粗控制
  • X-Y： Z方向细控制

这里并没有对旋转进行控制，其实实现也是很简单的。我们可以复用上面的按键或摇杆，设置移动和旋转两种模式，通过一个按钮在两种模式间进行切换即可。

import rospy
from moveit_msgs.msg import RobotState
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import InteractiveMarkerUpdate
from sensor_msgs.msg import  Joy
import moveit_commanderclass IMarkerJoystick:def __init__(self):self.pre_pose= PoseStamped()self.planning_groups_tips = {}self.ttySettings=termios.tcgetattr(sys.stdin)# 获得机器人的当前实际位置group_name = "arm"  # 这是对机器人进行moveit配置定义的关节组名称self.group = moveit_commander.MoveGroupCommander(group_name)self.robot = moveit_commander.RobotCommander()self.curPose=self.group.get_current_pose()self.initState= self.robot.get_current_state()# 创建相关self.goalState_pub = rospy.Publisher("/rviz/moveit/update_custom_goal_state",RobotState,queue_size=5)self.marker_pub = rospy.Publisher("/rviz/moveit/move_marker/goal_link_6",PoseStamped,queue_size=5)self.update_sub = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update",InteractiveMarkerUpdate,self.updateGoal)self.joy_sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)rospy.sleep(2)  # 确保rviz能收到消息# 将IMarker移动到与实际位置一致self.goalState_pub.publish(self.initState)rospy.sleep(1)# 回调函数通过接收update Topic来更新Marker的目标位置def updateGoal(self, msg):if len(msg.poses) > 0:self.curPose.pose.position = msg.poses[0].pose.positionself.curPose.pose.orientation = msg.poses[0].pose.orientationdef joyCB(self,msg):goalPose = self.curPosestatus = XBoxStatus(msg)print(status.select, status.start)print(status.circle, status.center, status.cross, status.square)# 粗调if status.left_analog_x <=-0.55 :goalPose.pose.position.x -=0.001elif status.left_analog_x >= 0.55 :goalPose.pose.position.x +=0.001if status.left_analog_y <= -0.55:goalPose.pose.position.y -= 0.001elif status.left_analog_y >= 0.55:goalPose.pose.position.y += 0.001if status.right_analog_y<=-0.55:goalPose.pose.position.z -=0.001if status.right_analog_y>=0.55:goalPose.pose.position.z += 0.001# 细调if status.left:goalPose.pose.position.x -=0.001if status.right:goalPose.pose.position.x +=0.001if status.up:goalPose.pose.position.y -=0.001if status.down:goalPose.pose.position.y +=0.001if status.square:goalPose.pose.position.z -=0.001if status.triangle:goalPose.pose.position.z +=0.001self.marker_pub.publish(goalPose)

这篇关于通过外部程序获得Moveit-Rviz的MotionPlanning面板相同的操控能力的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/443381。

相关文章

JAVA智听未来一站式有声阅读平台听书系统小程序源码

JAVA智听未来一站式有声阅读平台听书系统小程序源码

智听未来，一站式有声阅读平台听书系统 🌟&nbsp;开篇：遇见未来，从“智听”开始 在这个快节奏的时代，你是否渴望在忙碌的间隙，找到一片属于自己的宁静角落？是否梦想着能随时随地，沉浸在知识的海洋，或是故事的奇幻世界里？今天，就让我带你一起探索“智听未来”——这一站式有声阅读平台听书系统，它正悄悄改变着我们的阅读方式，让未来触手可及！ 📚&nbsp;第一站：海量资源，应有尽有 走进“智听
阅读更多...
spoj705( 求不相同的子串个数)

spoj705( 求不相同的子串个数)

题意：求串s的不同子串的个数 解题思路：任何子串都是某个后缀的前缀，对n个后缀排序，求某个后缀的前缀的个数，减去height[i](第i个后缀与第i-1 个后缀有相同的height[i]个前缀)。 代码如下： #include<iostream>#include<algorithm>#include<stdio.h>#include<math.h>#include<cstrin
阅读更多...
EMLOG程序单页友链和标签增加美化

EMLOG程序单页友链和标签增加美化

单页友联效果图： 标签页面效果图： 源码介绍 EMLOG单页友情链接和TAG标签，友链单页文件代码main{width: 58%;是设置宽度 自己把设置成与您的网站宽度一样，如果自适应就填写100%，TAG文件不用修改 安装方法：把Links.php和tag.php上传到网站根目录即可，访问 域名/Links.php、域名/tag.php 所有模板适用，代码就不粘贴出来，已经打
阅读更多...
跨系统环境下LabVIEW程序稳定运行

跨系统环境下LabVIEW程序稳定运行

在LabVIEW开发中，不同电脑的配置和操作系统（如Win11与Win7）可能对程序的稳定运行产生影响。为了确保程序在不同平台上都能正常且稳定运行，需要从兼容性、驱动、以及性能优化等多个方面入手。本文将详细介绍如何在不同系统环境下，使LabVIEW开发的程序保持稳定运行的有效策略。 LabVIEW版本兼容性 LabVIEW各版本对不同操作系统的支持存在差异。因此，在开发程序时，尽量使用
阅读更多...
EasyPlayer.js网页H5 Web js播放器能力合集

EasyPlayer.js网页H5 Web js播放器能力合集

最近遇到一个需求，要求做一款播放器，发现能力上跟EasyPlayer.js基本一致，满足要求： 需求 功性能 分类 需求描述 功能 预览 分屏模式 单分屏（单屏/全屏） 多分屏（2*2） 多分屏（3*3） 多分屏（4*4） 播放控制 播放（单个或全部） 暂停（暂停时展示最后一帧画面） 停止（单个或全部） 声音控制（开关/音量调节） 主辅码流切换 辅助功能 屏
阅读更多...
CSP 2023 提高级第一轮 CSP-S 2023初试题 完善程序第二题解析 未完

CSP 2023 提高级第一轮 CSP-S 2023初试题 完善程序第二题解析 未完

一、题目阅读 （最大值之和）给定整数序列 a0,⋯,an−1，求该序列所有非空连续子序列的最大值之和。上述参数满足 1≤n≤105 和 1≤ai≤108。 一个序列的非空连续子序列可以用两个下标 ll 和 rr（其中0≤l≤r<n0≤l≤r<n）表示，对应的序列为 al,al+1,⋯,ar​。两个非空连续子序列不同，当且仅当下标不同。 例如，当原序列为 [1,2,1,2] 时，要计算子序列 [
阅读更多...
这些心智程序你安装了吗？

这些心智程序你安装了吗？

原文题目：《为什么聪明人也会做蠢事（四）》 心智程序 大脑有两个特征导致人类不够理性，一个是处理信息方面的缺陷，一个是心智程序出了问题。前者可以称为“认知吝啬鬼”，前几篇文章已经讨论了。本期主要讲心智程序这个方面。 心智程序这一概念由哈佛大学认知科学家大卫•帕金斯提出，指个体可以从记忆中提取出的规则、知识、程序和策略，以辅助我们决策判断和解决问题。如果把人脑比喻成计算机，那心智程序就是人脑的
阅读更多...
使用JS/Jquery获得父窗口的几个方法(笔记)

使用JS/Jquery获得父窗口的几个方法(笔记)

<pre name="code" class="javascript">取父窗口的元素方法：$(selector, window.parent.document);那么你取父窗口的父窗口的元素就可以用：$(selector, window.parent.parent.document);如题： $(selector, window.top.document);//获得顶级窗口里面的元素 $(
阅读更多...
uniapp设置微信小程序的交互反馈

uniapp设置微信小程序的交互反馈

链接：uni.showToast(OBJECT) | uni-app官网 (dcloud.net.cn) 设置操作成功的弹窗： title是我们弹窗提示的文字 showToast是我们在加载的时候进入就会弹出的提示。 2.设置失败的提示窗口和标签 icon：'error'是设置我们失败的logo 设置的文字上限是7个文字，如果需要设置的提示文字过长就需要设置icon并给
阅读更多...
基于SpringBoot的宠物服务系统+uniapp小程序+LW参考示例

基于SpringBoot的宠物服务系统+uniapp小程序+LW参考示例

系列文章目录 1.基于SSM的洗衣房管理系统+原生微信小程序+LW参考示例 2.基于SpringBoot的宠物摄影网站管理系统+LW参考示例 3.基于SpringBoot+Vue的企业人事管理系统+LW参考示例 4.基于SSM的高校实验室管理系统+LW参考示例 5.基于SpringBoot的二手数码回收系统+原生微信小程序+LW参考示例 6.基于SSM的民宿预订管理系统+LW参考示例 7.基于
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2