def update_link(self): """ not working currently """ msg = LinkState() msg.link_name = "cart_front_steer::wheel_front_left_spin" msg.reference_frame = "world" # msg.twist.linear.x = 0.2 msg.twist.angular.z = 0.05 self.link_pub.publish(msg) msg.link_name = "cart_front_steer::wheel_front_right_spin" self.link_pub.publish(msg)
def talker(): pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10) rospy.init_node('dd_ctrl', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): buff = LinkState() buff.link_name = "dd_robot::left_wheel" buff.reference_frame = "dd_robot::chassis" buff.twist.linear.z = 10.0 buff.reference_frame = "dd_robot::left_wheel" buff.link_name = "dd_robot::chassis" rospy.loginfo(buff) pub.publish(buff) rate.sleep()
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 go_straight_ahead(des_pos): global pub, state_, z_back err_pos = math.sqrt( pow(des_pos.y - position_.y, 2) + pow(des_pos.x - position_.x, 2)) if des_pos.z != z_back: link_state_msg = LinkState() link_state_msg.link_name = "ball_link" link_state_msg.pose.position.x = position_.x link_state_msg.pose.position.y = position_.y link_state_msg.pose.position.z = des_pos.z z_back = des_pos.z pubz.publish(link_state_msg) if err_pos > dist_precision_: twist_msg = Twist() twist_msg.linear.x = kp_d * (des_pos.x - position_.x) if twist_msg.linear.x > ub_d: twist_msg.linear.x = ub_d elif twist_msg.linear.x < -ub_d: twist_msg.linear.x = -ub_d twist_msg.linear.y = kp_d * (des_pos.y - position_.y) if twist_msg.linear.y > ub_d: twist_msg.linear.y = ub_d elif twist_msg.linear.y < -ub_d: twist_msg.linear.y = -ub_d pub.publish(twist_msg) else: print('Position error: [%s]' % err_pos) change_state(1)
def drake_state_publisher(x_cmd): rate = rospy.Rate(100) times=0 xmsg=LinkState() while not rospy.is_shutdown(): xmsg.link_name='cartpole::link_chassis' # hello_str = "hello world %s" % rospy.get_time() buff=Pose position_buff=Point orientation_buff=Quaternion position_buff.x=x_cmd[times] position_buff.y=0 position_buff.z=0 orientation_buff.x=0 orientation_buff.y=0 orientation_buff.z=0 orientation_buff.w=1 buff.position=position_buff buff.orientation=orientation_buff #xmsg.pose = buffpose.position.x #xmsg.twist = [1,0,0,0,0,0] xmsg.pose.position.x = x_cmd[times] xmsg.pose.position.z = 2 drake_state_pub.publish(xmsg) rospy.loginfo(x_cmd[times]) drake_state_pub_echo.publish(x_cmd[times]) times=times+1 #'current x state is :{}'.format(statex) rate.sleep()
def generate_poses(n_cubes): valid_poses = False while not valid_poses: x_buff = np.around(np.random.uniform(-0.45, 0.45, (1, 3)), decimals=3) y_buff = np.around(np.random.uniform(-0.15, 0.23, (1, 3)), decimals=3) xy_buff = np.vstack((x_buff, y_buff)) d1 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 1]) d2 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 2]) d3 = np.linalg.norm(xy_buff[:, 1] - xy_buff[:, 2]) if d1 > 1.5 * CUBE_LENGTH and d2 > 1.5 * CUBE_LENGTH and \ d3 > 1.5 * CUBE_LENGTH: valid_poses = True new_row = [] for i in range(0, 3): target = LinkState() target.link_name = 'cube' + str(i + 1) + '_link' target.reference_frame = "table_surface_link" x = xy_buff[0, i] y = xy_buff[1, i] if i < n_cubes: z = CUBE_LENGTH / 4 + 0.0127 else: z = -0.2 yaw = round(random.uniform((pi * 3) / 5, (8 * pi / 5)), 5) quaternion = tf.transformations.quaternion_from_euler(pi, 0, yaw) new_row = new_row + [x, y, z] + list(quaternion) return new_row
def talker(): rospy.init_node('robot_pose_publisher', anonymous=True) robot_pose_pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10) robot_pose_msg = LinkState() # The name of the link, as in the model-1_4.sdf file. robot_pose_msg.link_name = 'taurob' # The robot is placed relative to the "map" frame. robot_pose_msg.reference_frame = 'map' listener = tf.TransformListener() # Updating the pose five times a second. rate = rospy.Rate(5) while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) robot_pose_msg.pose.position.x = trans[0] robot_pose_msg.pose.position.y = trans[1] robot_pose_msg.pose.orientation.z = rot[2] robot_pose_msg.pose.orientation.w = rot[3] robot_pose_pub.publish(robot_pose_msg) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue rate.sleep()
def generate_poses(p=0.3): valid_poses = False while not valid_poses: x_buff = np.around(np.random.uniform(-0.5, 0.5, (1, 3)), decimals=3) y_buff = np.around(np.random.uniform(-0.15, 0.25, (1, 3)), decimals=3) xy_buff = np.vstack((x_buff, y_buff)) d1 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 1]) d2 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 2]) d3 = np.linalg.norm(xy_buff[:, 1] - xy_buff[:, 2]) if d1 > 1.5 * CUBE_LENGTH and d2 > 1.5 * CUBE_LENGTH and d3 > 1.5 * CUBE_LENGTH: valid_poses = True print(xy_buff) target_poses = [] for i in range(0, 3): target = LinkState() target.link_name = 'cube' + str(i + 1) + '_link' target.reference_frame = "table_surface_link" x = xy_buff[0, i] y = xy_buff[1, i] prob_token = random.uniform(0, 1) if prob_token < p: z = -0.2 else: z = CUBE_LENGTH / 4 + 0.0127 yaw = round(random.uniform(pi / 2, 3 * pi / 2), 5) quaternion = tf.transformations.quaternion_from_euler(pi, 0, yaw) target.pose.position.x = x target.pose.position.y = y target.pose.position.z = z target.pose.orientation.x = quaternion[0] target.pose.orientation.y = quaternion[1] target.pose.orientation.z = quaternion[2] target.pose.orientation.w = quaternion[3] target_poses.append(target) return target_poses
def reset(self): self.pause() roll, pitch, yaw = np.random.random((3, )) * np.pi state = LinkState() quaternion = tf.transformations.quaternion_from_euler(0., 0., yaw) x, y = np.random.random((2, )) x *= 0.2 x += 1.1 y *= -0.5 y += -2 state.pose.position.x = x state.pose.position.y = y state.pose.position.z = 0.5 state.pose.orientation.x = quaternion[0] state.pose.orientation.y = quaternion[1] state.pose.orientation.z = quaternion[2] state.pose.orientation.w = quaternion[3] state.link_name = "base_link" #model_state = ModelState() #model_state.model_name = "mobile_base" #self.set_model_state(model_state=model_state) #self.set_link_state(link_state=state) self.index += 1 if self.index > 10: self.index = 0 self.unpause()
def setVelocity(linear=0, angular=0): if abs(linear) > 10: angular = copy.deepcopy(linear) angular = angular - 10 linear = 0 #a=GetLinkState._response_class() #a. linkStateIni = getLinkState('base_link') linkStateFin = LinkState() linkStateFin = copy.deepcopy(linkStateIni.link_state) #linkStateFin.twist=copy.deepcopy(linkStateIni.twist) linkStateFin.link_name = 'base_link' #print(linkState) service = '/gazebo/set_link_state' rospy.wait_for_service(service) setLinkState = rospy.ServiceProxy(service, SetLinkState) #modelState=ModelState() #factoryModelState(vl=[1,0,0]) #linkState=LinkState() linkStateFin.reference_frame = 'base_link' if linear != 0: linkStateFin.twist.linear.x = linear else: linkStateFin.twist.angular.z = angular resp = setLinkState(linkStateFin)
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 get_ball_home_link_state(): state_msg = LinkState() state_msg.link_name = 'ball_link' state_msg.reference_frame = 'wrist_3_link' state_msg.pose.position.x = 0 state_msg.pose.position.y = 0.2 state_msg.pose.position.z = 0 state_msg.pose.orientation.x = 0 state_msg.pose.orientation.y = 0 state_msg.pose.orientation.z = 0 state_msg.pose.orientation.w = 0 return state_msg
def change_link_state(t, quat): try: f = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) state = LinkState() state.link_name = 'chessboard::chessboard' state.pose.position = Point(t[0], t[1], t[2]) state.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) status = f(state) print(status) except rospy.ServiceException as e: print('Service call failed: %s' % e)
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 setmodel(): pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) pub1 = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10) rospy.init_node('set_model_state', anonymous=True) rate = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): modelstate = get_state_call("crumb") #print modelstate cmd = ModelState() link = LinkState() link.link_name = "wheel_left_link" link.twist.angular.y = 2 link.reference_frame = "wheel_left_link" link1 = LinkState() link1.link_name = "wheel_right_link" link1.twist.angular.y = 2 link1.reference_frame = "wheel_right_link" pub1.publish(link) #pub1.publish(link1) print link cmd.pose.position = modelstate.pose.position cmd.twist.linear.x = 0 cmd.twist.linear.y = 0 cmd.twist.linear.z = 0 cmd.twist.angular.x = 0.0 cmd.twist.angular.y = 0.0 cmd.twist.angular.z = 0 cmd.model_name = "crumb" #print cmd #pub.publish(cmd) rate.sleep()
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 read_cube_pose_file(self, sample_idx): cube_poses = self.cube_pose_file.iloc[sample_idx, :].values # print(cube_poses) cube_poses = cube_poses.reshape((3, -1)) target_poses = [] for i in range(0, 3): target = LinkState() target.link_name = 'cube' + str(i + 1) + '_link' target.reference_frame = "table_surface_link" target.pose.position.x = cube_poses[i, 0] target.pose.position.y = cube_poses[i, 1] target.pose.position.z = cube_poses[i, 2] target.pose.orientation.x = cube_poses[i, 3] target.pose.orientation.y = cube_poses[i, 4] target.pose.orientation.z = cube_poses[i, 5] target.pose.orientation.w = cube_poses[i, 6] target_poses.append(target) return target_poses
def reset_client(self, angle): q = tf.transformations.quaternion_from_euler(0.0, angle, 0.0) request = LinkState() request.link_name = "myrrbot7::myrrbot7_link2" request.pose.position.x = 0.0 + 2.0 request.pose.position.y = 0.1 - 2.0 request.pose.position.z = 1.45 request.pose.orientation.x = q[0] request.pose.orientation.y = q[1] request.pose.orientation.z = q[2] request.pose.orientation.w = q[3] # print "request.pose : " # print request.pose try: reset = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) resp = reset(request) # print "resp : ", resp except rospy.ServiceException, e: print "Service call faild : %s" % e
def move_cube_to_target(self, link_name, target_pose): rospy.wait_for_service('/gazebo/set_link_state') target = LinkState() target.link_name = link_name target.reference_frame = "link" target.pose.position.x = target_pose[0] target.pose.position.y = target_pose[1] target.pose.position.z = target_pose[2] target.pose.orientation.x = target_pose[3] target.pose.orientation.y = target_pose[4] target.pose.orientation.z = target_pose[5] target.pose.orientation.w = target_pose[6] try: set_cube_position = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) move_status = set_cube_position(target) return move_status.success except rospy.ServiceException, e: print "Service call failed: %s"
def go_straight_ahead(des_pos): global pub, state_, z_back, message if des_pos.z >= 0: #in case the ball appears in the arena message = "start play" ## publisher that publishes a message whenever is given the command for which the ball is in the arena pub_start_play = rospy.Publisher("/ball/chatter", String, queue_size=10) pub_start_play.publish(message) err_pos = math.sqrt( pow(des_pos.y - position_.y, 2) + pow(des_pos.x - position_.x, 2)) if des_pos.z != z_back: link_state_msg = LinkState() link_state_msg.link_name = "ball_link" link_state_msg.pose.position.x = position_.x link_state_msg.pose.position.y = position_.y link_state_msg.pose.position.z = des_pos.z z_back = des_pos.z pubz.publish(link_state_msg) if err_pos > dist_precision_: twist_msg = Twist() twist_msg.linear.x = kp_d * (des_pos.x - position_.x) if twist_msg.linear.x > ub_d: twist_msg.linear.x = ub_d elif twist_msg.linear.x < -ub_d: twist_msg.linear.x = -ub_d twist_msg.linear.y = kp_d * (des_pos.y - position_.y) if twist_msg.linear.y > ub_d: twist_msg.linear.y = ub_d elif twist_msg.linear.y < -ub_d: twist_msg.linear.y = -ub_d pub.publish(twist_msg) else: print('Position error: [%s]' % err_pos) change_state(1)
def talker(): pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10) ppp = LinkState() rospy.init_node('talker', anonymous=True) rate = rospy.Rate(100) # 10hz i = 1 while not rospy.is_shutdown(): ppp.link_name = "platform" ppp.pose.position.x = 0.1 ppp.pose.position.y = 0.1 ppp.pose.position.z = 1 ppp.pose.orientation.x = 0 ppp.pose.orientation.y = 0 ppp.pose.orientation.z = 0 ppp.pose.orientation.w = 0 i = i + 1 rospy.loginfo(ppp) pub.publish(ppp) rate.sleep()
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 go_straight_ahead(des_pos): """ Moves the ball towards the specified coordinates Computes the distance to the goal, and moves the ball accordingly. Limits the velocity if needed, and checks for the yaw error. Once it arrives changes to state 1 """ global pub, state_, z_back err_pos = math.sqrt(pow(des_pos.y - position_.y, 2) + pow(des_pos.x - position_.x, 2)) if des_pos.z != z_back: link_state_msg = LinkState() link_state_msg.link_name = "ball_link" link_state_msg.pose.position.x = position_.x link_state_msg.pose.position.y = position_.y link_state_msg.pose.position.z = des_pos.z z_back = des_pos.z pubz.publish(link_state_msg) if err_pos > dist_precision_: twist_msg = Twist() twist_msg.linear.x = kp_d * (des_pos.x-position_.x) if twist_msg.linear.x > ub_d: twist_msg.linear.x = ub_d elif twist_msg.linear.x < -ub_d: twist_msg.linear.x = -ub_d twist_msg.linear.y = kp_d * (des_pos.y-position_.y) if twist_msg.linear.y > ub_d: twist_msg.linear.y = ub_d elif twist_msg.linear.y < -ub_d: twist_msg.linear.y = -ub_d pub.publish(twist_msg) else: #print ('Position error: [%s]' % err_pos) change_state(1)
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
def change_pose(): rospy.init_node('set_pose') state_msg = LinkState() state_msg.link_name = 'box' #Setting the required pose state_msg.pose.position.x = -2.0 state_msg.pose.position.y = -1.0 state_msg.pose.position.z = 0 state_msg.pose.orientation.x = 0 state_msg.pose.orientation.y = 0 state_msg.pose.orientation.z = 0 state_msg.pose.orientation.w = 0 rospy.wait_for_service('/gazebo/set_link_state') try: set_state = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) #Publishing the message resp = set_state(state_msg) except rospy.ServiceException, e: print "Service call failed: %s" % e
# topic = "/gazebo_11355/set_link_state" topic = "/gazebo/set_link_state" pub = rospy.Publisher(topic, LinkState, queue_size=1) rospy.init_node('change_obj_pose', anonymous=True) rospy.wait_for_service('/gazebo/set_link_state') # rospy.wait_for_service('/gazebo_11355/set_link_state') print("service is ready") serv = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) # serv = rospy.ServiceProxy('/gazebo_11355/set_link_state', SetLinkState) state = LinkState() #state.link_name = "cylinderHandle::left_wheel" state.link_name = "shapetoExplore3::sphere1" # state.pose.position.x = -0.59 # state.pose.position.y = - 0.1 # state.pose.position.z = 0.07 x = -0.59 y = -0.1 z = 0.07 x_range = 0.1 y_range = 0.1 state.pose.position.x = random.uniform(x - x_range, x + x_range) state.pose.position.y = random.uniform(y - y_range, y + y_range) state.pose.position.z = z print(state.pose.position) z_rot = random.uniform(-pi / 4, pi / 4) print(z_rot)
serv = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) # serv = rospy.ServiceProxy('/gazebo_11355/set_link_state', SetLinkState) state = LinkState() numobj = range(3) rotation_list = random.sample(numobj, len(numobj)) for j in range(3): object_list = random.sample(numobj, len(numobj)) for i in range(0, 3): for k in range(50): print('\033[;40m ') object_tograsp = object_list[i] state.pose.position.x = 0.5 state.pose.position.y = 0.5 state.pose.position.z = 0.5 state.link_name = "cylinderHandle::left_wheel" sstate = SetLinkStateRequest() sstate.link_state = state serv(sstate) time.sleep(0.5) state.pose.position.x = 0.7 state.pose.position.y = 0.7 state.pose.position.z = 0.7 state.link_name = "cross_joint_part::link" sstate = SetLinkStateRequest() sstate.link_state = state serv(sstate) time.sleep(0.5) state.pose.position.x = 0.9 state.pose.position.y = 0.9 state.pose.position.z = 0.9
rate = rospy.Rate(1) radius = 3.0 time_period = 12.0 t = 0 traj = OrbitTrajectory(radius, time_period) print 'Waiting for Gazebo. Start the roslaunch?' rospy.wait_for_service('/gazebo/set_link_state') set_link_service = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) rospy.wait_for_service('/gazebo/set_physics_properties') pause_physics_service = rospy.ServiceProxy('/gazebo/pause_physics', Empty) pause_physics_service() state = LinkState() state.pose.position.x = -3 state.link_name = 'simple_rgbd_line_cam::link1' state.reference_frame = 'world' state.pose.orientation.w = 1 xs = [] ys = [] while not rospy.is_shutdown(): try: pose = traj.get_current_pose(t) pos = pose[:3, 3] quat = tf.transformations.quaternion_from_matrix(pose) state.pose.position.x = pos[0] state.pose.position.y = pos[1] state.pose.position.z = pos[2]
def set_track_speeds(lefttrack_speed, righttrack_speed): global old_lefttrack_speed global old_righttrack_speed global old_lefttrack_position global old_righttrack_position global old_timestamp # Compute deltas and new track positions new_timestamp = rospy.get_time() delta_t = new_timestamp - old_timestamp #print("Delta time [ms]: " + "{:.0f}".format(1000*delta_t)) lefttrack_position = old_lefttrack_position + 0.5 * ( old_lefttrack_speed + lefttrack_speed) * delta_t righttrack_position = old_righttrack_position + 0.5 * ( old_righttrack_speed + righttrack_speed) * delta_t #print("Left track position [m]: "+"{:.4f}".format(lefttrack_position)) # Iterate old_timestamp = new_timestamp old_lefttrack_position = lefttrack_position old_righttrack_position = righttrack_position old_lefttrack_speed = lefttrack_speed old_righttrack_speed = righttrack_speed # Setting the new state of the wheels leftwheels_newlinkstate = LinkState() rightwheels_newlinkstate = LinkState() leftwheels_newlinkstate.pose.position = initial_wheel_state.pose.position rightwheels_newlinkstate.pose.position = initial_wheel_state.pose.position leftwheels_quaternion = tf_transform.quaternion_from_euler( lefttrack_position / 0.030, 0, 0, axes='syxz') leftwheels_newlinkstate.pose.orientation.x = leftwheels_quaternion[0] leftwheels_newlinkstate.pose.orientation.y = leftwheels_quaternion[1] leftwheels_newlinkstate.pose.orientation.z = leftwheels_quaternion[2] leftwheels_newlinkstate.pose.orientation.w = leftwheels_quaternion[3] rightwheels_quaternion = tf_transform.quaternion_from_euler( righttrack_position / 0.030, 0, 0, axes='syxz') rightwheels_newlinkstate.pose.orientation.x = rightwheels_quaternion[0] rightwheels_newlinkstate.pose.orientation.y = rightwheels_quaternion[1] rightwheels_newlinkstate.pose.orientation.z = rightwheels_quaternion[2] rightwheels_newlinkstate.pose.orientation.w = rightwheels_quaternion[3] leftwheels_newlinkstate.twist.angular.y = lefttrack_speed / 0.030 rightwheels_newlinkstate.twist.angular.y = righttrack_speed / 0.030 #TODO replace wheel radius with ros parameter try: #print("You got the service handle !") # Repeat for each of the 8 legs for ii in range(4): leftwheels_newlinkstate.link_name = "link_wheel" + str(ii + 1) leftwheels_newlinkstate.reference_frame = "link_leg" + str(ii + 1) rightwheels_newlinkstate.link_name = "link_wheel" + str(ii + 5) rightwheels_newlinkstate.reference_frame = "link_leg" + str(ii + 5) rospy.wait_for_service('/gazebo/set_link_state') set_link_state = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) leftresponse = set_link_state(leftwheels_newlinkstate) rospy.wait_for_service('/gazebo/set_link_state') set_link_state = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) rightresponse = set_link_state(rightwheels_newlinkstate) #print("Last left wheel response: "+leftresponse.status_message) return True except rospy.ServiceException as e: print("Service call failed: %s" % e)