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 object_picking(self): global PICKING if PICKING: angle = quaternion_from_euler(1.57, 0.0, 0.0) object_picking = LinkState() object_picking.link_name = self.string object_picking.pose = Pose(self.model_pose_picking.position, self.model_pose_picking.orientation) object_picking.reference_frame = "wrist_3_link" self.pub_model_position.publish(object_picking)
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