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