在rviz中仿真和实际使用,直接用github里的直接可以用。官方链接
官方缺少gazebo的描述文件和使用,在下面这个链接有使用gazebo仿真
上面这个也有问题,虽然在gazebo中能看到模型,但是不能控制其动作,因此用到了gazebo插件,链接如下gazebo插件
在配置好gazebo模型和控制插件后,进一步判断gazebo中是如何抓取的。
<forces_angle_tolerance>100</forces_angle_tolerance>
<update_rate>100</update_rate>
<grip_count_threshold>2</grip_count_threshold>
<max_grip_count>8</max_grip_count>
<release_tolerance>0.01</release_tolerance> <!-- 0.01977<0.0198<0.01999 -->
<disable_collisions_on_attach>false</disable_collisions_on_attach>
<contact_topic>__default_topic__</contact_topic>
#include <gazebo/transport/transport.hh>
#include <gazebo_grasp_plugin/msgs/grasp_event.pb.h>
#include <gazebo_grasp_plugin_ros/GazeboGraspEvent.h>
#include <ros/ros.h>
using GraspEventPtr = boost::shared_ptr<const gazebo::msgs::GraspEvent>;
ros::Publisher gGraspEventPublisher;
void ReceiveGraspMsg(const GraspEventPtr& gzMsg)
{
ROS_INFO_STREAM("Re-publishing grasp event: " << gzMsg->DebugString());
gazebo_grasp_plugin_ros::GazeboGraspEvent rosMsg;
rosMsg.arm = gzMsg->arm();
rosMsg.object = gzMsg->object();
rosMsg.attached = gzMsg->attached();
gGraspEventPublisher.publish(rosMsg);
}
int main(int argc, char** argv)
{
// Initialize ROS and Gazebo transport
ros::init(argc, argv, "gazebo_grasp_plugin_event_republisher");
if (!gazebo::transport::init())
{
ROS_ERROR("Unable to initialize gazebo transport - is gazebo running?");
return 1;
}
gazebo::transport::run();
// Subscribe to Gazebo grasp event message
gazebo::transport::NodePtr gzNode(new gazebo::transport::Node());
gzNode->Init();
gazebo::transport::SubscriberPtr subscriber;
try
{
subscriber = gzNode->Subscribe("~/grasp_events", &ReceiveGraspMsg);
}
catch (std::exception e)
{
ROS_ERROR_STREAM("Error subscribing to topic: " << e.what());
return 1;
}
// Initialize ROS publisher
ros::NodeHandle rosNode("~");
const std::string pubTopic = "grasp_events";
gGraspEventPublisher =
rosNode.advertise<gazebo_grasp_plugin_ros::GazeboGraspEvent>(pubTopic, 1);
ros::spin();
gazebo::transport::fini();
return 0;
}
使用: rosrun gazebo_grasp_plugin_ros grasp_event_republisher
#! /usr/bin/env python
# -*- coding: UTF-8 -*-
import rospy
from gazebo_grasp_plugin_ros.msg import GazeboGraspEvent
def doMsg(p):
print(p.attached)
if(p.attached):
rospy.loginfo("Attached grasp target!")
if __name__ == "__main__":
#1.初始化节点
rospy.init_node("GazeboGraspMsgRead")
#2.创建订阅者对象
sub = rospy.Subscriber("/gazebo_grasp_plugin_event_republisher/grasp_events", GazeboGraspEvent, doMsg, queue_size=1)
rospy.spin() #4.循环
下面是通过gazebo下控制robotiq实现抓取一个位置宽度的胶带。
在启动程序中出现绿色字体日志,显示抓取吸附了,下面个两个终端是我们分别用gazebo信号转ROS话题并发布和订阅显示已经抓取成功。