def linkStatesCb(self, msg): ''' Callback for gazebo link states @param msg: received message @type msg: gazebo_msgs/LinkStates ''' for i in range(len(msg.name)): model_id = msg.name[i].split('::')[0] # Adds dynamically all the links for all the models if model_id in self._gazebo_robots: link_state = LinkState() link_state.link_name = msg.name[i] link_state.pose = msg.pose[i] link_state.twist = msg.twist[i] # already exist? if link_state.link_name in self._gazebo_robots[model_id][ 'links']: self._gazebo_robots[model_id]['links'][ link_state.link_name].update(link_state, rospy.Time.now()) else: rospy.loginfo('%s::linkStatesCb: adding new link %s', self.node_name, link_state.link_name) self._gazebo_robots[model_id]['links'][ link_state.link_name] = GazeboLinkState() self._gazebo_robots[model_id]['links'][ link_state.link_name].update(link_state, rospy.Time.now()) elif model_id in self._gazebo_objects: link_state = LinkState() link_state.link_name = msg.name[i] link_state.pose = msg.pose[i] link_state.twist = msg.twist[i] # already exist? if link_state.link_name in self._gazebo_objects[model_id][ 'links']: self._gazebo_objects[model_id]['links'][ link_state.link_name].update(link_state, rospy.Time.now()) else: rospy.loginfo('%s::linkStatesCb: adding new link %s', self.node_name, link_state.link_name) self._gazebo_objects[model_id]['links'][ link_state.link_name] = GazeboLinkState() self._gazebo_objects[model_id]['links'][ link_state.link_name].update(link_state, rospy.Time.now())
def main(): global last_attach_link, force_on new_link_state = LinkState() force = Wrench() pose = Point() new_link_state.reference_frame = 'palletizer_robot::prisos_point' #rate = rospy.Rate(100) dists = [] centre_box = [] while not rospy.is_shutdown(): if (not vacuum_on): dists, centre_box = find_dists(pose_cubs_list, pos_orient_prisos) if (force_on): force.force.z = 0 force_srv(last_attach_link, 'world', pose, force, rospy.Time.from_sec(0), rospy.Duration.from_sec(-1.0)) force_on = False else: id_grasp_cub = find_aviable_cub(centre_box) if (not id_grasp_cub == -1): new_link_state.link_name = link_name[id_cubs[id_grasp_cub]] force.force.z = 0.01 last_attach_link = link_name[id_cubs[id_grasp_cub]] dist = dists[id_grasp_cub] new_link_state.pose.orientation.x = dist[3] new_link_state.pose.orientation.y = dist[4] new_link_state.pose.orientation.z = dist[5] new_link_state.pose.orientation.w = dist[6] new_link_state.pose.position.x = dist[0] new_link_state.pose.position.y = dist[1] new_link_state.pose.position.z = dist[2] + 0.003 new_link_state.twist = twist pub.publish(new_link_state) if (not force_on): force_srv(last_attach_link, 'world', pose, force, rospy.Time.from_sec(0), rospy.Duration.from_sec(-1.0)) force_on = True else: dists, centre_box = find_dists(pose_cubs_list, pos_orient_prisos) if (force_on): force_srv(last_attach_link, 'world', pose, force, rospy.Time.from_sec(0), rospy.Duration.from_sec(-1.0)) force_on = False #rate.sleep() time.sleep(0.04)
def links_cb(self, data): pos = None try: index = data.name.index(self.robot_base_name) pos = data.pose[index] vel = data.twist[index] except Exception as e: print e if pos: ls = LinkState() ls.reference_frame = "/world" ls.pose = pos ls.twist = vel ls.link_name = self.robot_base_name self.pub.publish(ls)
def setPoseOfObjectToPickUp(self, pose): # sets pose and twist of a link. All children link poses/twists of the URDF tree are not updated accordingly, but should be. newLinkState = LinkState() # link name, link_names are in gazebo scoped name notation, [model_name::body_name] newLinkState.link_name = "objectToPickUp::objectToPickUp_link" newLinkState.pose = pose # desired pose in reference frame newTwist = Twist() newTwist.linear.x = 0.0 newTwist.linear.y = 0.0 newTwist.linear.z = 0.0 newTwist.angular.x = 0.0 newTwist.angular.y = 0.0 newTwist.angular.z = 0.0 newLinkState.twist = newTwist # desired twist in reference frame # set pose/twist relative to the frame of this link/body, leave empty or "world" or "map" defaults to world-frame newLinkState.reference_frame = "world" self.link_state_pub.publish(newLinkState)
def bat(self, name): def callback(link_msg): self.links = link_msg self.j = 5 i = 0 while self.links.name[i] != name: i = i + 1 self.current_pose = self.links.pose[i] self.current_twist = self.links.twist[i] for i in range(20000): sub = rospy.Subscriber("/gazebo/link_states", LinkStates, callback) batting = LinkState() batting.pose = self.current_pose batting.twist = self.current_twist batting.link_name = name batting.pose.orientation.y = batting.pose.orientation.y + 0.4 print(self.current_pose) print(batting) self.pub_link.publish(batting)
def shoot(self): print("Shooting") if self.ammo > 0: #spawn projectile rospy.wait_for_service("gazebo/spawn_sdf_model") spawn = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel) projectile_path = os.getcwd() + "/maps/projectile.sdf" projectile_name = "projectile" + str(13 - self.ammo) pose = Pose() posePos = Point() posePos.x = self.position[0] posePos.y = self.position[1] posePos.z = self.position[2] poseAng = Quaternion() poseAng.x = 0 poseAng.y = 0 poseAng.z = 0 poseAng.w = 1 pose.position = posePos pose.orientation = poseAng pose.position.z += 0.25 f = open(projectile_path) sdf = f.read() spawn(projectile_name, sdf, "", pose, "") link_name = projectile_name + "::projectile_link" twist = Twist() velocity = 3.0 twist.linear.x = velocity * np.cos(self.zangle * np.pi / 180) twist.linear.y = velocity * np.sin(self.zangle * np.pi / 180) ls = LinkState() ls.link_name = link_name ls.pose = pose ls.twist = twist rospy.wait_for_service("gazebo/set_link_state") set_state = rospy.ServiceProxy("gazebo/set_link_state", SetLinkState) set_state(ls) self.ammo = self.ammo - 1