Python geometry_msgs.msg.PoseStamped() Examples
The following are 30
code examples of geometry_msgs.msg.PoseStamped().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
geometry_msgs.msg
, or try the search function
.
Example #1
Source File: carla_waypoint_publisher.py From ros-bridge with MIT License | 21 votes |
def publish_waypoints(self): """ Publish the ROS message containing the waypoints """ msg = Path() msg.header.frame_id = "map" msg.header.stamp = rospy.Time.now() if self.current_route is not None: for wp in self.current_route: pose = PoseStamped() pose.pose.position.x = wp[0].transform.location.x pose.pose.position.y = -wp[0].transform.location.y pose.pose.position.z = wp[0].transform.location.z quaternion = tf.transformations.quaternion_from_euler( 0, 0, -math.radians(wp[0].transform.rotation.yaw)) pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] msg.poses.append(pose) self.waypoint_publisher.publish(msg) rospy.loginfo("Published {} waypoints.".format(len(msg.poses)))
Example #2
Source File: multi_goals.py From tianbot_racecar with GNU General Public License v3.0 | 8 votes |
def __init__(self, goalListX, goalListY, retry, map_frame): self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10) self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) # params & variables self.goalListX = goalListX self.goalListY = goalListY self.retry = retry self.goalId = 0 self.goalMsg = PoseStamped() self.goalMsg.header.frame_id = map_frame self.goalMsg.pose.orientation.z = 0.0 self.goalMsg.pose.orientation.w = 1.0 # Publish the first goal time.sleep(1) self.goalMsg.header.stamp = rospy.Time.now() self.goalMsg.pose.position.x = self.goalListX[self.goalId] self.goalMsg.pose.position.y = self.goalListY[self.goalId] self.pub.publish(self.goalMsg) rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) self.goalId = self.goalId + 1
Example #3
Source File: omnirobot_server.py From robotics-rl-srl with MIT License | 7 votes |
def visualRobotCallback(self, pose_stamped_msg): """ Callback for ROS topic Get the new updated position of robot from camera :param pose_stamped_msg: (PoseStamped ROS message) """ self.robot_pos[0] = pose_stamped_msg.pose.position.x self.robot_pos[1] = pose_stamped_msg.pose.position.y self.robot_yaw = euler_from_quaternion([pose_stamped_msg.pose.orientation.x, pose_stamped_msg.pose.orientation.y, pose_stamped_msg.pose.orientation.z, pose_stamped_msg.pose.orientation.w])[2] if NO_TARGET_MODE and self.target_pos_changed: # simulate the target's position update self.target_pos[0] = 0.0 self.target_pos[1] = 0.0 self.target_yaw = 0.0 self.target_pos_changed = False
Example #4
Source File: tasks.py From flyingros with GNU General Public License v3.0 | 7 votes |
def setpoint_init(self): # type_mask # 2552 : XYZ & yaw - POSITION # 7104 : XYZ, yaw, vXYZ, TAKE_OFF_SETPOINT # 3064 : 0000 1001 1111 1000 self.setpoint_raw = PositionTarget() self.setpoint_raw.coordinate_frame = self.setpoint_raw.FRAME_LOCAL_NED self.setpoint_raw.type_mask = self.type_mask_Fly self.setpoint_raw.position = Point() self.setpoint_raw.yaw = 0.0 self.setpoint_raw.velocity = Vector3() self.setpoint_raw.acceleration_or_force = Vector3() self.setpoint_raw.yaw_rate = 0.0 self.setpoint_local = PoseStamped() self.setpoint_local.pose.orientation.w = 1 self.laser_position_count = 0
Example #5
Source File: ros_agent.py From scenario_runner with MIT License | 7 votes |
def publish_plan(self): """ publish the global plan """ msg = Path() msg.header.frame_id = "/map" msg.header.stamp = rospy.Time.now() for wp in self._global_plan_world_coord: pose = PoseStamped() pose.pose.position.x = wp[0].location.x pose.pose.position.y = -wp[0].location.y pose.pose.position.z = wp[0].location.z quaternion = tf.transformations.quaternion_from_euler( 0, 0, -math.radians(wp[0].rotation.yaw)) pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] msg.poses.append(pose) rospy.loginfo("Publishing Plan...") self.waypoint_publisher.publish(msg)
Example #6
Source File: pick_and_place_working_1.py From ROS-Programming-Building-Powerful-Robots with MIT License | 7 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #7
Source File: pick_and_place_pick_working.py From ROS-Programming-Building-Powerful-Robots with MIT License | 7 votes |
def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.005)) return p.pose
Example #8
Source File: pick_and_place_working_1.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #9
Source File: pick_and_place_working_1.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #10
Source File: pick_and_place_pick_working.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #11
Source File: pick_and_place_working_1.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.009)) return p.pose
Example #12
Source File: pick_and_place.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.45 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #13
Source File: pick_and_place_both_working_good.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #14
Source File: pick_and_place_both_working_good.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.03, 0.03, 0.009)) return p.pose
Example #15
Source File: pick_and_place_ew.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.4 p.pose.position.y = 0.0 p.pose.position.z = 0.26 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #16
Source File: pick_and_place_ew.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.03, 0.03, 0.09)) return p.pose
Example #17
Source File: pick_and_place_pick_working.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #18
Source File: pick_and_place_both_working_good.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #19
Source File: pick_and_place_working_1.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.009)) return p.pose
Example #20
Source File: pick_and_place.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.45 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #21
Source File: pick_and_place_both_working_good.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.03, 0.03, 0.009)) return p.pose
Example #22
Source File: task_generator.py From drl_local_planner_ros_stable_baselines with BSD 3-Clause "New" or "Revised" License | 6 votes |
def __publish_goal(self, x, y, theta): """ Publishing goal (x, y, theta) :param x x-position of the goal :param y y-position of the goal :param theta theta-position of the goal """ self.__old_path_stamp = self.__path.header.stamp goal = PoseStamped() goal.header.stamp = rospy.get_rostime() goal.header.frame_id = "map" goal.pose.position.x = x goal.pose.position.y = y quaternion = self.__yaw_to_quat(theta) goal.pose.orientation.w = quaternion[0] goal.pose.orientation.x = quaternion[1] goal.pose.orientation.y = quaternion[2] goal.pose.orientation.z = quaternion[3] self.__goal_pub_.publish(goal) return
Example #23
Source File: planning_scene_interface.py From kinova-movo with BSD 3-Clause "New" or "Revised" License | 6 votes |
def addCylinder(self, name, height, radius, x, y, z, wait=True): s = SolidPrimitive() s.dimensions = [height, radius] s.type = s.CYLINDER ps = PoseStamped() ps.header.frame_id = self._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.pose.orientation.w = 1.0 self.addSolidPrimitive(name, s, ps.pose, wait) ## @brief Insert new box into planning scene ## @param name Name of the object ## @param size_x The x-dimensions size of the box ## @param size_y The y-dimensions size of the box ## @param size_z The z-dimensions size of the box ## @param x The x position in link_name frame ## @param y The y position in link_name frame ## @param z The z position in link_name frame ## @param wait When true, we wait for planning scene to actually update, ## this provides immunity against lost messages.
Example #24
Source File: omnirobot_server.py From robotics-rl-srl with MIT License | 6 votes |
def visualTargetCallback(self, pose_stamped_msg): """ Callback for ROS topic Get the new updated position of robot from camera Only update when target position should have been moved (eg. reset env) :param pose_stamped_msg: (PoseStamped ROS message) """ if self.target_pos_changed: if pose_stamped_msg.pose.position.x < TARGET_MAX_X and pose_stamped_msg.pose.position.x > TARGET_MIN_X \ and pose_stamped_msg.pose.position.y > TARGET_MIN_Y and pose_stamped_msg.pose.position.y < TARGET_MAX_Y: self.target_pos[0] = pose_stamped_msg.pose.position.x self.target_pos[1] = pose_stamped_msg.pose.position.y self.target_yaw = euler_from_quaternion([pose_stamped_msg.pose.orientation.x, pose_stamped_msg.pose.orientation.y, pose_stamped_msg.pose.orientation.z, pose_stamped_msg.pose.orientation.w])[2] self.target_pos_changed = False
Example #25
Source File: control_thread.py From flyingros with GNU General Public License v3.0 | 6 votes |
def sendSetpoint(): # Input data global setpoint, yawSetPoint, run, position_control # Output data local_setpoint_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=0) rate = rospy.Rate(5) while run: q = tf.transformations.quaternion_from_euler(0, 0, deg2radf(yawSetPoint), axes="sxyz") msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.pose.position.x = float(setpoint.x) msg.pose.position.y = float(setpoint.y) msg.pose.position.z = float(setpoint.z) msg.pose.orientation.x = q[0] msg.pose.orientation.y = q[1] msg.pose.orientation.z = q[2] msg.pose.orientation.w = q[3] local_setpoint_pub.publish(msg) rate.sleep()
Example #26
Source File: pick_and_place_pick_working.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
Example #27
Source File: mavros_control2.py From vision_to_mavros with GNU General Public License v3.0 | 6 votes |
def __init__(self): rospy.init_node("mav_control_node") rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback) rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback) self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1) self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1) self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1) # mode 0 = STABILIZE # mode 4 = GUIDED # mode 9 = LAND self.mode_service = rospy.ServiceProxy('/mavros/set_mode', SetMode) self.arm_service = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.takeoff_service = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) self.rc = RCIn() self.pose = Pose() self.timestamp = rospy.Time()
Example #28
Source File: trajectory_planner.py From TrajectoryPlanner with MIT License | 6 votes |
def __init__(self): self.map = None self.start = None self.goal = None self.moves = [Move(0.1, 0), # forward Move(-0.1, 0), # back Move(0, 1.5708), # turn left 90 Move(0, -1.5708)] # turn right 90 self.robot = Robot(0.5, 0.5) self.is_working = False # Replace with mutex after all self.map_subscriber = rospy.Subscriber("map", OccupancyGrid, self.new_map_callback) self.start_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.new_start_callback) self.goal_subscriber = rospy.Subscriber("goal", PoseStamped, self.new_goal_callback) self.path_publisher = rospy.Publisher("trajectory", MarkerArray, queue_size=1) self.pose_publisher = rospy.Publisher("debug_pose", PoseStamped, queue_size=1) # what will be there. A module goes into variable. Isn't it too much memory consumption. Maybe I should assign function replan() to this variable? self.planner = planners.astar.replan
Example #29
Source File: mavros_control1.py From vision_to_mavros with GNU General Public License v3.0 | 6 votes |
def __init__(self): rospy.init_node("mav_control_node") rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback) rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback) self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1) self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1) self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1) # mode 0 = STABILIZE # mode 4 = GUIDED # mode 9 = LAND self.mode_service = rospy.ServiceProxy('/mavros/set_mode', SetMode) self.arm_service = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.takeoff_service = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) self.rc = RCIn() self.pose = Pose() self.timestamp = rospy.Time()
Example #30
Source File: pick_and_place_both_working_good.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose