Python geometry_msgs.msg.TwistStamped() Examples
The following are 11
code examples of geometry_msgs.msg.TwistStamped().
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: lidarEvasion.py From Cherry-Autonomous-Racecar with MIT License | 6 votes |
|
def __init__(self):
self.evadeSet = False
self.controller = XBox360()
self.bridge = CvBridge()
self.throttle = 0
self.grid_img = None
##self.throttleLock = Lock()
print "evasion"
rospy.Subscriber("/lidargrid", Image, self.gridCB, queue_size=1)
rospy.Subscriber("/cmd_vel", TwistStamped , self.twistCB , queue_size = 1)
rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5)
self.pub_img = rospy.Publisher("/steering_img", Image)
self.pub_twist = rospy.Publisher("/lidar_twist", TwistStamped, queue_size=1)
self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
rospy.init_node ('lidar_cmd',anonymous=True)
rospy.spin()
Example #2
| Source File: dataRecorder.py From Cherry-Autonomous-Racecar with MIT License | 6 votes |
|
def __init__(self):
print "dataRecorder"
self.record = False
self.twist = None
self.twistLock = Lock()
self.bridge = CvBridge()
self.globaltime = None
self.controller = XBox360()
##rospy.Subscriber("/camera/depth/points" , PointCloud2, self.ptcloudCB)
rospy.Subscriber("/camera/depth/image_rect_raw_drop", Image, self.depthCB)
rospy.Subscriber("/camera/rgb/image_rect_color_drop", Image, self.streamCB)
#rospy.Subscriber("/camera/rgb/image_rect_mono_drop", Image, self.streamCB) ##for black and white images see run.launch and look at the drop fps nodes near the bottom
rospy.Subscriber("/lidargrid", Image, self.lidargridCB)
rospy.Subscriber("/cmd_vel", TwistStamped, self.cmd_velCB)
rospy.Subscriber("/joy", Joy ,self.joyCB)
self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
rospy.init_node('dataRecorder',anonymous=True)
rospy.spin()
Example #3
| Source File: runModel.py From Cherry-Autonomous-Racecar with MIT License | 6 votes |
|
def __init__(self):
rospy.loginfo("runner.__init__")
# ----------------------------
self.sess = tf.InteractiveSession()
self.model = cnn_cccccfffff()
saver = tf.train.Saver()
saver.restore(self.sess, "/home/ubuntu/catkin_ws/src/car/scripts/model.ckpt")
rospy.loginfo("runner.__init__: model restored")
# ----------------------------
self.bridge = CvBridge()
self.netEnable = False
self.controller = XBox360()
rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.runCB)
rospy.Subscriber("/joy", Joy ,self.joyCB)
self.pub_twist = rospy.Publisher("/neural_twist", TwistStamped, queue_size=1)
self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
rospy.init_node('neural_cmd',anonymous=True)
rospy.spin()
Example #4
| Source File: controller_node.py From ros2cli with Apache License 2.0 | 5 votes |
|
def __init__(self):
super().__init__('controller')
self.pub = self.create_publisher(
TwistStamped, 'cmd_vel', qos_profile_system_default
)
self.tmr = self.create_timer(1.0, self.callback)
Example #5
| Source File: controller_node.py From ros2cli with Apache License 2.0 | 5 votes |
|
def callback(self):
msg = TwistStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.twist.linear.x = 1.0
self.pub.publish(msg)
Example #6
| Source File: lidarEvasion.py From Cherry-Autonomous-Racecar with MIT License | 5 votes |
|
def twistCB(self, cmd_vel):
if self.evadeSet == True:
try:
##rospy.loginfo("cmd_vel Recieved")
self.throttle = cmd_vel.twist.linear.x
normed_throttle = (self.throttle*2.0)-1.0
front_max = 0.3 + 4.5*(normed_throttle**2.5) ##front region scales with throttle value
rospy.loginfo('normed_throttle: '+str(normed_throttle) + ' front_max: '+str(front_max))
front = self.Occupancy(self.grid_img, 0.1, front_max, -0.2, 0.2) ##(2,0.2) to (0.5,-0.2)
right = self.Occupancy(self.grid_img, 0.0, 1, -0.7, -0.2) ##(2,-0.2) to (0,-0.7)
left = self.Occupancy(self.grid_img, 0.0, 1, 0.2, 0.7) ##(2,0.7) to (0,0.2)
everywhere = self.Occupancy(self.grid_img, -4.0, 4.0, -4.0, 4.0)
cmd = TwistStamped()
#rospy.loginfo(self.throttle)
cmd.twist.angular.z = 0.5
cmd.twist.linear.x = -1.0
if front:
cmd.twist.linear.x = 0.5 ##stop
self.pub_twist.publish(cmd)
self.sound.publish("Forward collision detected")
elif left:
cmd.twist.angular.z = 0.7 ##turn right
self.pub_twist.publish(cmd)
self.sound.publish("Left collision detected")
elif right:
cmd.twist.angular.z = 0.3 ##turn left
self.pub_twist.publish(cmd)
self.sound.publish("Right collision detected")
else:
#self.pub_twist.publish(cmd)
pass
except Exception as f:
print(f)
else:
pass
##rospy.loginfo("Not using Evasion")
Example #7
| Source File: cmdControl.py From Cherry-Autonomous-Racecar with MIT License | 5 votes |
|
def __init__(self):
self.throttleInitialized = False
self.joy_timeout = 2.0
self.lid_timeout = 0.30
self.cnn_timeout = 0.1
self.joy_time = time.time()
self.lid_time = time.time()
self.cnn_time = time.time()
self.controller = XBox360()
self.joy_cmd = TwistStamped()
self.lid_cmd = TwistStamped()
self.cnn_cmd = TwistStamped()
self.cruiseControl = False
self.cruiseThrottle = 0.5
self.steeringAngle = 0.5
self.throttle = 0.5
self.trim = 0.0
##self.throttleLock = Lock()
print "cmd_control"
rospy.Subscriber("/imu", Imu, self.imuCB, queue_size=5)
rospy.Subscriber("/lidar_twist", TwistStamped, self.lidarTwistCB, queue_size=5)
rospy.Subscriber("/neural_twist", TwistStamped, self.neuralTwistCB, queue_size=5)
rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5)
self.vel_pub = rospy.Publisher("/cmd_vel", TwistStamped, queue_size = 1)
#self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=1)
self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
rospy.init_node ('cmd_control',anonymous=True)
rate = rospy.Rate(66)
while not rospy.is_shutdown():
self.cmdRouter()
rate.sleep()
Example #8
| Source File: kitti2bag.py From kitti2bag with MIT License | 5 votes |
|
def save_gps_vel_data(bag, kitti, gps_frame_id, topic):
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
twist_msg = TwistStamped()
twist_msg.header.frame_id = gps_frame_id
twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
twist_msg.twist.linear.x = oxts.packet.vf
twist_msg.twist.linear.y = oxts.packet.vl
twist_msg.twist.linear.z = oxts.packet.vu
twist_msg.twist.angular.x = oxts.packet.wf
twist_msg.twist.angular.y = oxts.packet.wl
twist_msg.twist.angular.z = oxts.packet.wu
bag.write(topic, twist_msg, t=twist_msg.header.stamp)
Example #9
| Source File: kitti2bag.py From SARosPerceptionKitti with MIT License | 5 votes |
|
def save_gps_vel_data(bag, kitti, gps_frame_id, topic):
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
twist_msg = TwistStamped()
twist_msg.header.frame_id = gps_frame_id
twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
twist_msg.twist.linear.x = oxts.packet.vf
twist_msg.twist.linear.y = oxts.packet.vl
twist_msg.twist.linear.z = oxts.packet.vu
twist_msg.twist.angular.x = oxts.packet.wf
twist_msg.twist.angular.y = oxts.packet.wl
twist_msg.twist.angular.z = oxts.packet.wu
bag.write(topic, twist_msg, t=twist_msg.header.stamp)
Example #10
| Source File: body2ned.py From correlation_flow with GNU General Public License v3.0 | 4 votes |
|
def toNED(msg):
# fliter measured velocity
global vx_p, vy_p
vx = 1*msg.twist.linear.x + 0*vx_p
vy = 1*msg.twist.linear.y + 0*vy_p
v_body = array([vx, -vy, 0])
# transform body velocity to ENU
global q
[qx, qy, qz, qw] = [q[0], q[1], q[2], q[3]]
Tenu = array([[1-2*qy*qy-2*qz*qz, 2*qx*qy-2*qz*qw, 2*qx*qz+2*qy*qw],
[2*qx*qy + 2*qz*qw, 1-2*qx*qx-2*qz*qz, 2*qy*qz-2*qx*qw],
[2*qx*qz-2*qy*qw , 2*qy*qz + 2*qx*qw, 1-2*qx*qx-2*qy*qy]])
v = dot(Tenu, v_body)
# ENU to NED: (x,y,z) -> (x,-y,-z)
twist = TwistStamped()
twist.header = Header()
twist.header.frame_id = "ned"
twist.header.stamp = rospy.Time.now()
twist.twist.linear.x = v[0]
twist.twist.linear.y = -v[1]
twist.twist.linear.z = 0
pub.publish(twist)
vx_p = vx
vy_p = vy
# record data in vicon frame, compare with vicon
q_ned_vicon = tf.transformations.quaternion_from_euler(math.pi, 0, -yaw)
[qx, qy, qz, qw] = [q_ned_vicon[0], q_ned_vicon[1], q_ned_vicon[2], q_ned_vicon[3]]
Tv = array([[1-2*qy*qy-2*qz*qz, 2*qx*qy-2*qz*qw, 2*qx*qz+2*qy*qw],
[2*qx*qy + 2*qz*qw, 1-2*qx*qx-2*qz*qz, 2*qy*qz-2*qx*qw],
[2*qx*qz-2*qy*qw , 2*qy*qz + 2*qx*qw, 1-2*qx*qx-2*qy*qy]])
vr = dot(Tv, array([v[0],-v[1],0]))
outtxt.write(str.format("{0:.9f} ", msg.header.stamp.to_sec()))
outtxt.write(str.format("{0:.9f} ", vx))
outtxt.write(str.format("{0:.9f} ", vy))
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0\n')
Example #11
| Source File: state_collector.py From drl_local_planner_ros_stable_baselines with BSD 3-Clause "New" or "Revised" License | 3 votes |
|
def __init__(self, ns, train_mode):
# Class variables
self.__mode = train_mode # Mode of RL-agent (Training or Executrion ?)
self.__ns = ns # namespace of simulation environment
self.__is__new = False # True, if waypoint reached
self.__static_scan = LaserScan() # Laserscan only contains static objects
self.__ped_scan = LaserScan() # Laserscan only contains pedestrians
self.__f_scan = LaserScan()
self.__f_scan.header.frame_id = "base_footprint"
self.__b_scan = LaserScan()
self.__b_scan.header.frame_id = "base_footprint"
self.__img = OccupancyGrid() # input image
self.__wps= Waypoint() # most recent Waypoints
self.__twist = TwistStamped() # most recent velocity
self.__goal = Pose() # most recent goal position in robot frame
self.__state_mode = 2 # 0, if image as input state representation
# 1, if stacked image representation in same frame
# 2, if scan as input state representation
# Subscriber
self.wp_sub_ = rospy.Subscriber("%s/wp" % (self.__ns), Waypoint, self.__wp_callback, queue_size=1)
if ["train", "eval"].__contains__(self.__mode):
# Info only avaible during evaluation and training
self.wp_sub_reached_ = rospy.Subscriber("%s/wp_reached" % (self.__ns), Waypoint, self.__wp_reached_callback, queue_size=1)
self.static_scan_sub_ = rospy.Subscriber("%s/static_laser" % (self.__ns), LaserScan, self.__static_scan_callback,
queue_size=1)
self.ped_scan_sub_ = rospy.Subscriber("%s/ped_laser" % (self.__ns), LaserScan, self.__ped_scan_callback,
queue_size=1)
self.twist_sub_ = rospy.Subscriber("%s/twist" % (self.__ns), TwistStamped, self.__twist_callback, queue_size=1)
self.goal_sub_ = rospy.Subscriber("%s/rl_agent/robot_to_goal" % (self.__ns), PoseStamped, self.__goal_callback, queue_size=1)
else:
self.static_scan_sub_ = rospy.Subscriber("%s/b_scan" % (self.__ns), LaserScan,
self.__b_scan_callback,
queue_size=1)
self.static_scan_sub_ = rospy.Subscriber("%s/f_scan" % (self.__ns), LaserScan,
self.__f_scan_callback,
queue_size=1)
# Service
self.__img_srv = rospy.ServiceProxy('%s/image_generator/get_image' % (self.__ns), StateImageGenerationSrv)
self.__sim_in_step = rospy.ServiceProxy('%s/is_in_step' % (self.__ns), SetBool)
self.__merge_scans = rospy.ServiceProxy('%s/merge_scans' % (self.__ns), MergeScans)
