def reset(self): print "reset" rospy.wait_for_service('/gazebo/reset_world') try: reset_world() except(rospy.ServiceException) as e: print "reset_world failed!" rospy.wait_for_service('/gazebo/reset_simulation') try: reset_simulation() except(rospy.ServiceException) as e: print "reset_simulation failed!" reset_cube = ModelState() reset_cube.model_name = "cube2" reset_cube.reference_frame = "world" reset_cube.pose.position.x = 0 reset_cube.pose.position.y = 0 reset_cube.pose.position.z = 0 reset_cube.pose.orientation.x = 0 reset_cube.pose.orientation.y = 0 reset_cube.pose.orientation.z = 0 reset_cube.twist.linear.x = 0 reset_cube.twist.linear.y = 0 reset_cube.twist.linear.z = 0 set_model = SetModelState() set_model.request.model_state = reset_cube # rospy.wait_for_service("cube2", [0, 0, 0],[0, 0, 0], ) # '{model_state: { model_name: cube2, pose: { position: { x: 0, y: 0 ,z: 1 }, orientation: {x: 0, y: 0.491983115673, z: 0, w: 0.870604813099 } }, twist: { linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }' rospy.wait_for_service('/gazebo/set_model_configuration') try: reset_joints("robot", "robot_description", ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], [1.0,-1.8,0.0, 0.0, 0.0, 0.0]) except (rospy.ServiceException) as e: print "reset_joints failed!" rospy.wait_for_service('/gazebo/pause_physics') try: pause() except (rospy.ServiceException) as e: print "rospause failed!" rospy.wait_for_service('/gazebo/unpause_physics') try: unpause() except (rospy.ServiceException) as e: print "/gazebo/pause_physics service call failed" print "reset done."
def set_position(self, position): model_state_resp = self.get_model_state(model_name="mobile_base") model_state = SetModelState() model_state.model_name = "mobile_base" model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose = model_state_resp.pose model_state.pose.position.x = position[0] model_state.pose.position.y = position[1] self.set_model_state(model_state=model_state)
def position_indicator(self): state = SetModelState() state.model_name = "indicator" pose = Pose() pose.position.x = self.center[0] pose.position.y = (-1) * self.center[1] pose.position.z = (-1) * self.center[2] state.pose = pose state.twist = Twist() state.reference_frame = "" self.setModelState(state)
def set_position(self, position): model_state_resp = self.get_model_state(model_name="piano2") model_state = SetModelState() model_state.model_name = "piano2" model_state.pose = model_state_resp.pose model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose.position.x = position.X model_state.pose.position.y = position.Y model_state.pose.position.z = position.Z self.set_model_state(model_state=model_state)
def teleport(self, location): """ Teleport robot's position. location: string location name """ if location == 0: print("\teleporting to lobby") else: print("\teleporting to room {}".format(location)) location = self.numbered_locations[location] # get location data lx = self.locations[location]["lx"] ly = self.locations[location]["ly"] az = 0.0 if "az" in self.locations[location]: az = self.locations[location]["az"] # send location data to service rospy.wait_for_service("/gazebo/set_model_state") set_model_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) model_state = SetModelState() model_state.model_name = "mobile_base" pose = Pose() pose.position.x = lx pose.position.y = ly orientation = quaternion_from_euler(0, 0, az) pose.orientation.x = orientation[0] pose.orientation.y = orientation[1] pose.orientation.z = orientation[2] pose.orientation.w = orientation[3] model_state.pose = pose twist = Twist() twist.linear.x = 0.0 twist.angular.z = 0.0 model_state.twist = twist model_state.reference_frame = "map" set_model_state(model_state) print("reached {}\n".format(location))
def set_steering_angle(self, quat): model_state_resp = self.get_model_state(model_name="piano2") model_state = SetModelState() model_state.model_name = "piano2" model_state.pose = model_state_resp.pose model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose.orientation.x = quat[0] model_state.pose.orientation.y = quat[1] model_state.pose.orientation.z = quat[2] model_state.pose.orientation.w = quat[3] self.set_model_state(model_state=model_state)
def set_heading_angle(self, heading_angle): model_state_resp = self.get_model_state(model_name="mobile_base") model_state = SetModelState() model_state.model_name = "mobile_base" model_state.pose = model_state_resp.pose model_state.twist = Twist() model_state.reference_frame = "world" quat = tf.transformations.quaternion_from_euler(0, 0, heading_angle) model_state.pose.orientation.x = quat[0] model_state.pose.orientation.y = quat[1] model_state.pose.orientation.z = quat[2] model_state.pose.orientation.w = quat[3] self.set_model_state(model_state=model_state)
def set_state(self, se3): model_state_resp = self.get_model_state(model_name="piano2") model_state = SetModelState() model_state.model_name = "piano2" model_state.pose = model_state_resp.pose model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose.position.x = se3.X model_state.pose.position.y = se3.Y model_state.pose.position.z = se3.Z model_state.pose.orientation.x = se3.q[0] model_state.pose.orientation.y = se3.q[1] model_state.pose.orientation.z = se3.q[2] model_state.pose.orientation.w = se3.q[3] self.set_model_state(model_state=model_state)
def set_state(self, x, y, s, vx=0, vy=0, vs=0): model_state_resp = self.get_model_state(model_name="ackermann_vehicle") model_state = SetModelState() model_state.model_name = "ackermann_vehicle" model_state.pose = model_state_resp.pose model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose.position.x = x model_state.pose.position.y = y quat = tf.transformations.quaternion_from_euler(0,0,s) model_state.pose.orientation.x = quat[0] model_state.pose.orientation.y = quat[1] model_state.pose.orientation.z = quat[2] model_state.pose.orientation.w = quat[3] model_state.twist.linear.x = vx model_state.twist.linear.y = vy model_state.twist.angular.z = vs self.set_model_state(model_state=model_state)
def __init__(self): #-------point cloud without color------- rospy.loginfo("Start to set gazebo pose") self.set_gazebo_srv = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) self.save_image_srv = rospy.ServiceProxy("/save_image", Trigger) self.state = ModelState() self.robot_state = ModelState() self.set_model_state = SetModelState() self.theta_range = (-90, 90) self.distance_range = (3, 7.5) self.yaw_range = (0, 360) self.counter = 0 self.object_list = ['person_walking', 'person_walking_clone', 'person_walking_clone_0', 'person_walking_clone_1',\ 'person_walking_clone_2', 'person_walking_clone_3', 'person_walking_clone_4',\ 'person_standing', 'person_standing_clone', 'person_standing_clone_0', 'person_standing_clone_1'\ , 'person_standing_clone_2'] while(True): self.set_gazebo_pose() rospy.sleep(2.5) self.save_image() rospy.sleep(1.5)
def moveModel(model, position, r, p, y): try: service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) twist = Twist() pose = Pose() pose.position = position #rpy to quaternion: quat = quaternion_from_euler(r, p, y) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] req = SetModelState() req.model_name = model req.pose = pose req.twist = twist req.reference_frame = "" resp = service(req) except rospy.ServiceException, e: print("Service call failed: %s" %e)
def _reset(self): if self.proc is not None: os.system("rosnode kill /rviz") os.system("rosnode kill /move_base") os.system("rosnode kill /pcl_filter") rospy.sleep(5) state = SetModelState() state.model_name = 'mobile_base' state.pose = self.start state.twist = Twist() state.reference_frame = '' rospy.wait_for_service('/gazebo/set_model_state') try: self.set_model_state(state) except (rospy.ServiceException) as e: print("/gazebo/set_model_state service call failed") reset = String() reset.data = "r" self.reset_pub.publish(reset) rospy.sleep(3) self.goal = Pose() self.goal.position.x = random.random() * ( self.map_size_x_max - self.map_size_x_min) + self.map_size_x_min self.goal.position.y = random.random() * ( self.map_size_y_max - self.map_size_y_min) + self.map_size_x_min self.goal.orientation.w = 1 print(self.goal.position) # Unpause simulation to make observation rospy.wait_for_service('/gazebo/unpause_physics') try: #resp_pause = pause.call() self.unpause() except (rospy.ServiceException) as e: print("/gazebo/unpause_physics service call failed") #try to initialize SLAM vel_cmd = Twist() vel_cmd.linear.x = -0.4 r = rospy.Rate(10) count = 0 status_ = Bool() while status_.data is False and count < 10: t = time.time() if count % 5 is 0: vel_cmd.linear.x = -vel_cmd.linear.x #rospy.sleep(2) while time.time() - t < 2.0: self.vel_pub.publish(vel_cmd) r.sleep() status_ = rospy.wait_for_message('/ORB_SLAM2/Status', Bool, timeout=5) print(status_) count += 1 rospy.sleep(1) # what will happen if SLAM is still not initialized if status_ is False: state = self._reset() return state rospy.sleep(2) #self.port = os.environ["ROS_PORT_SIM"] self.proc = subprocess.Popen([ "roslaunch", "-p", self.port, "/home/rrc/gym-gazebo/gym_gazebo/envs/assets/launch/navigation_stack_rl.launch" ]) rospy.sleep(5) goal_published = PoseStamped() goal_published.pose = self.goal goal_published.header.frame_id = "odom" self.goal_pub.publish(goal_published) rospy.sleep(1) goal_done_ = self.goal_done dist_ = self.dist costmap_data_ = self.costmap_data collision_ = self.collision feature_ = None while feature_ is None: try: feature_ = rospy.wait_for_message('/ORB_SLAM2/FeatureInfo', Bool, timeout=5) except: pass # pause simulation rospy.wait_for_service('/gazebo/pause_physics') try: #resp_pause = pause.call() self.pause() except (rospy.ServiceException) as e: print("/gazebo/pause_physics service call failed") temp1 = np.asarray( [self.prev_cmd_vel.linear.x, self.prev_cmd_vel.angular.z, dist_]) temp2 = np.asarray(feature_.data) temp3 = np.asarray(costmap_data_) state = np.concatenate((temp1, temp2, temp3), axis=0) return state
import rospy from geometry_msgs.msg import Point32, Twist, Pose from matplotlib import pyplot as plt from gp_abstract_sim.msg import path_points from nav_msgs.msg import Odometry from tf.transformations import euler_from_quaternion from gazebo_msgs.msg import ModelState from math import sqrt, pow, atan2 from std_msgs.msg import Float32 from gazebo_msgs.srv import GetModelState, SetModelState ########################################################################### speed = Twist() state_msg = ModelState() gestate = GetModelState() sestate = SetModelState() new_data = Odometry() current_x = 0.0 current_y = 0.0 current_yaw = 0.0 flagdamn = False flag = False gx = 0.0 gy = 0.0 gyaw = 0.0 currentPath = path_points() size = 0 delta_x = 0.0 delta_y = 0.0
def setModelState(modelStateObj): return SetModelState(model_state=modelStateObj)