def move_callback(self, msg):
self.thrust = msg.linear.x
self.thrust_zeroing_counter = 0
acc = Accel()
acc.linear = msg.linear
acc.angular = msg.angular
self.accelPub.publish(acc)
def __init__(self):
self.max_preservation_time = 30.0
self.accel_alpha = 0.95
self.dt = 0.02
self.last_odom = Odometry()
self.last_vel = None
self.last_accel = None
self.sub_odom = None
self.sub_cmd_acc = None
self.sub_cmd_vel = None
self.sub_cmd_pos = None
self.last_cmd_pos = PoseStamped()
self.last_cmd_vel = Twist()
self.last_cmd_acc = Accel()
self.startup_time = rospy.Time.now().to_sec()
self.responses = [r.value for r in ResponseType]
self.buffers = {
r: [0.0] * int(ceil((self.max_preservation_time / self.dt)))
for r in ResponseType
}
for r in self.responses:
buf = [0] * int(ceil(1.0 / self.dt * self.max_preservation_time))
self.buffers[r] = buf
rospy.Timer(rospy.Duration(self.dt), self.update)
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
from geometry_msgs.msg import Accel
self.accel = kwargs.get('accel', Accel())
def publishAccel(self, stamp, states_der):
accelMsg = Accel()
accelMsg.linear.x = states_der[4]
accelMsg.linear.y = states_der[5]
accelMsg.linear.z = 0.0
accelMsg.angular.x = 0.0
accelMsg.angular.y = 0.0
accelMsg.angular.z = states_der[6]
self.accel_pub.publish(accelMsg)
def _parse_joy(self, joy=None):
if self._msg_type == 'twist':
cmd = Twist()
else:
cmd = Accel()
if joy is not None:
# Linear velocities:
l = Vector3(0, 0, 0)
if self._axes['x'] > -1 and abs(joy.axes[self._axes['x']]) > self._deadzone:
l.x += self._axes_gain['x'] * joy.axes[self._axes['x']]
if self._axes['y'] > -1 and abs(joy.axes[self._axes['y']]) > self._deadzone:
l.y += self._axes_gain['y'] * joy.axes[self._axes['y']]
if self._axes['z'] > -1 and abs(joy.axes[self._axes['z']]) > self._deadzone:
l.z += self._axes_gain['z'] * joy.axes[self._axes['z']]
if self._axes['xfast'] > -1 and abs(joy.axes[self._axes['xfast']]) > self._deadzone:
l.x += self._axes_gain['xfast'] * joy.axes[self._axes['xfast']]
if self._axes['yfast'] > -1 and abs(joy.axes[self._axes['yfast']]) > self._deadzone:
l.y += self._axes_gain['yfast'] * joy.axes[self._axes['yfast']]
if self._axes['zfast'] > -1 and abs(joy.axes[self._axes['zfast']]) > self._deadzone:
l.z += self._axes_gain['zfast'] * joy.axes[self._axes['zfast']]
# Angular velocities:
a = Vector3(0, 0, 0)
if self._axes['roll'] > -1 and abs(joy.axes[self._axes['roll']]) > self._deadzone:
a.x += self._axes_gain['roll'] * joy.axes[self._axes['roll']]
if self._axes['rollfast'] > -1 and abs(joy.axes[self._axes['rollfast']]) > self._deadzone:
a.x += self._axes_gain['rollfast'] * joy.axes[self._axes['rollfast']]
if self._axes['pitch'] > -1 and abs(joy.axes[self._axes['pitch']]) > self._deadzone:
a.y += self._axes_gain['pitch'] * joy.axes[self._axes['pitch']]
if self._axes['pitchfast'] > -1 and abs(joy.axes[self._axes['pitchfast']]) > self._deadzone:
a.y += self._axes_gain['pitchfast'] * joy.axes[self._axes['pitchfast']]
if self._axes['yaw'] > -1 and abs(joy.axes[self._axes['yaw']]) > self._deadzone:
a.z += self._axes_gain['yaw'] * joy.axes[self._axes['yaw']]
if self._axes['yawfast'] > -1 and abs(joy.axes[self._axes['yawfast']]) > self._deadzone:
a.z += self._axes_gain['yawfast'] * joy.axes[self._axes['yawfast']]
cmd.linear = l
cmd.angular = a
else:
cmd.linear = Vector3(0, 0, 0)
cmd.angular = Vector3(0, 0, 0)
return cmd
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from geometry_msgs.msg import Accel
self.accel = kwargs.get('accel', Accel())
if 'covariance' not in kwargs:
self.covariance = numpy.zeros(36, dtype=numpy.float64)
else:
self.covariance = numpy.array(kwargs.get('covariance'), dtype=numpy.float64)
assert self.covariance.shape == (36, )
def publish(self):
accelgyro = self.mpu.read_accelgyros()
# Set acceleration data and gyro data (Normalize for PLEN Axis)
response = Accel()
response.linear.x = accelgyro[0]
response.linear.y = -accelgyro[1]
response.linear.z = -accelgyro[2]
response.angular.x = accelgyro[3]
response.angular.y = -accelgyro[4]
response.angular.z = -accelgyro[5]
self.publisher.publish(response)
def publishAccel(self, stamp, states_dot):
"""
State index and definition
states_dot : [x_dot, y_dot, yaw_dot, vx_dot]
"""
accelMsg = Accel()
accelMsg.linear.x = states_dot[3]
accelMsg.linear.y = 0.0
accelMsg.linear.z = 0.0
accelMsg.angular.x = 0.0
accelMsg.angular.y = 0.0
accelMsg.angular.z = 0.0
self.accel_pub.publish(accelMsg)
def send_thrust(self):
command = self.vector
msg = Accel()
# msg.header = Header()
# #msg.header.stamp = rospy.Time.now()
# msg.header.frame_id = self.output_frame
msg.linear.x = command[0]
msg.linear.y = command[1]
msg.linear.z = command[2]
msg.angular.x = command[3]
msg.angular.y = command[4]
msg.angular.z = command[5]
self.pub.publish(msg)
def carla_acceleration_to_ros_accel(carla_acceleration):
"""
Convert carla acceleration to a ROS accel
Considers the conversion from the left-handed system (unreal) to the right-handed system (ROS)
Considers that the angular accelerations remain zero.
:param carla_acceleration: The Carla Acceleration
:type carla_acceleration: carla.Vector3D
:return: a ROS accel
:rtype: geometry_msgs.msg.Accel
"""
ros_accel = Accel()
ros_accel.linear.x = carla_acceleration.x
ros_accel.linear.y = -carla_acceleration.y
ros_accel.linear.z = carla_acceleration.z
return ros_accel
def carla_acceleration_to_icv_accel(carla_acceleration):
"""
Convert a carla acceleration to a icv accel
Considers the conversion from left-handed system (unreal) to right-handed
system (icv)
The angular accelerations remain zero.
:param carla_acceleration: the carla acceleration
:type carla_acceleration: carla.Vector3D
:return: a icv accel
:rtype: geometry_msgs.msg.Accel
"""
icv_accel = Accel()
icv_accel.linear.x = carla_acceleration.x
icv_accel.linear.y = -carla_acceleration.y
icv_accel.linear.z = carla_acceleration.z
return icv_accel
def __init__(self):
self.robot_vel_publisher = rospy.Publisher('/cmd_vel',
Twist,
queue_size=1)
self.robot_position_publisher = rospy.Publisher('positionTopic',
Point,
queue_size=1)
self.robot_orientation_publisher = rospy.Publisher('positionTopic',
Vector3,
queue_size=1)
self.robot_acceleration_publisher = rospy.Publisher('positionTopic',
Accel,
queue_size=1)
self.cmd = Twist()
self.ctrl_c = False
self.rate = rospy.Rate(1)
self.position = Point()
self.orientation = Vector3()
self.accel = Accel()
rospy.on_shutdown(self.shutdownhook)
def _parse_joy(self, joy=None):
if self._msg_type == 'twist':
cmd = Twist()
else:
cmd = Accel()
if joy is not None:
cmd.linear = Vector3(
self._axes_gain['x'] *
(joy.axes[self._axes['x']]
if abs(joy.axes[self._axes['x']]) > 0.5 else 0.0),
self._axes_gain['y'] *
(joy.axes[self._axes['y']]
if abs(joy.axes[self._axes['y']]) > 0.5 else 0.0),
self._axes_gain['z'] *
(joy.axes[self._axes['z']]
if abs(joy.axes[self._axes['z']]) > 0.5 else 0.0))
cmd.angular = Vector3(
0, 0, self._axes_gain['yaw'] *
(joy.axes[self._axes['yaw']]
if abs(joy.axes[self._axes['yaw']]) > 0.5 else 0.0))
else:
cmd.linear = Vector3(0, 0, 0)
cmd.angular = Vector3(0, 0, 0)
return cmd
def __init__(self):
self.prefix = ""
self.first_call = True
dt = 1.0/120.0
w = 10 * 3.141592
w_acc = 50 * 3.141592
w_angular = 4 * 3.141592
w_angular_acc = 4 * 3.141592
self.origin_x = -8.5
self.max_slip = 0
self.max_speed = 0
self.cur_speed = 0
self.cur_speed_b = 0
self.cur_slip = 0
self.lap_time_avg = 0
self.lap_time_std = 0
self.max_speed = 0
self.avg_speed = 0
self.x_lpf = LPF(dt,w,0.0)
self.y_lpf = LPF(dt,w,0.0)
self.z_lpf = LPF(dt,w,0.0)
self.vx_lpf = LPF(dt,w,0.0)
self.vy_lpf = LPF(dt,w,0.0)
self.vz_lpf = LPF(dt,w,0.0)
self.ax = 0.0
self.ay = 0.0
self.az = 0.0
self.ax_lpf = LPF(dt,w_acc,0.0)
self.ay_lpf = LPF(dt,w_acc,0.0)
self.az_lpf = LPF(dt,w_acc,0.0)
self.xa = 0.0
self.ya = 0.0
self.va = 0.0
self.xb = 0.0
self.yb = 0.0
self.vb = 0.0
self.roll = 0.0
self.roll_rate = 0.0
self.pitch = 0.0
self.pitch_rate = 0.0
self.yaw = 0.0
self.yaw_rate = 0.0
self.heading_multiplier = 0.0
self.roll_lpf = LPF(dt,w_angular,0.0)
self.pitch_lpf = LPF(dt,w_angular,0.0)
self.yaw_lpf = LPF(dt,10*w_angular,0.0)
self.roll_rate_lpf = LPF(dt,w_angular,0.0)
self.pitch_rate_lpf = LPF(dt,w_angular,0.0)
self.yaw_rate_lpf = LPF(dt,w_angular,0.0)
self.roll_rate_rate_lpf = LPF(dt,w_angular_acc,0.0)
self.pitch_rate_rate_lpf = LPF(dt,w_angular_acc,0.0)
self.yaw_rate_rate_lpf = LPF(dt,w_angular_acc,0.0)
self.seq = 1
self.cur_pose_odom = Odometry()
self.last_pose_time = 0.0
self.last_pose_msg = PoseStamped()
self.last_roll = 0.0
self.last_roll_rate = 0.0
self.last_pitch = 0.0
self.last_pitch_rate = 0.0
self.last_yaw = 0.0
self.last_yaw_temp = 0.0
self.last_yaw_rate = 0.0
self.last_v_x = 0.0
self.last_v_y = 0.0
self.last_v_z = 0.0
rospy.Subscriber("pose", PoseStamped, self.poseSubCallback)
# rospy.Subscriber("ground_pose", Pose2D, self.groundPoseSubCallback)
# rospy.Subscriber("mppi_controller/subscribedPose", Odometry, self.mppiposeCallback)
# rospy.Subscriber("lap_stats", pathIntegralStats, self.lapStatCallback)
self.speed_pub = rospy.Publisher('info/linear_speed', Float64, queue_size = 1)
self.slip_pub = rospy.Publisher("info/slip_angle", Float64, queue_size=1)
self.distance_pub = rospy.Publisher("info/distance", Float64, queue_size=1)
self.ttc_pub = rospy.Publisher("info/ttc", Float64, queue_size=1)
self.angle_pub = rospy.Publisher("info/angle_difference", Float64, queue_size=1)
self.yaw_pub = rospy.Publisher("info/yaw", Float64, queue_size=1)
self.yaw_rate_pub = rospy.Publisher("info/yaw_rate", Float64, queue_size=1)
self.pose_odom_pub = rospy.Publisher("pose_estimate", Odometry, queue_size=1)
self.accel_pub_x = rospy.Publisher("acceleration/x", Float64, queue_size=1)
self.accel_pub_y = rospy.Publisher("acceleration/y", Float64, queue_size=1)
self.accel_pub_z = rospy.Publisher("acceleration/z", Float64, queue_size=1)
self.accel_pub_all = rospy.Publisher("acceleration/all", Accel, queue_size=1)
self.accelMsg = Accel()
self.br = tf2_ros.TransformBroadcaster()
self.t = TransformStamped()
class SI(object):
''' Data for the Turtlebot'''
'''
turtlebot_pose = Pose2D() # TODO: PoseWithCovariance
turtlebot_vel = Pose2D()
turtlebot_acc = Accel() # TODO: AccelWithCovariance
'''
# pose of turtle bot
tb_pos = (0,0,0)
tb_ang = Quaternion()
# left and right wheel
angLeft = 0.0 # positive direction: clockwise: Left - Right
angRight = 0.0
angAbs = 0.0 # Absolute rotation angle
# Velocity of TB
tb_vel = Pose2D() # Pose2D: float64 x, float64 y, float64 theta
freq = 10.0 # Frequency of updating velocity: 10Hz
posX = [0.0]*2 # frequency of /tf: 60Hz
posY = [0.0]*2 # [old, new]
posT = [0.0]*2 # for angular velocity
# Command (to publish)
tb_cmd = Pose2D() # X_vel, Y_vel, anguler_vel
# TODO: PoseArray: Header header, Pose[] poses
''' Data for the Ball '''
ball_pose = Pose2D()
ball_vel = Pose2D()
ball_acc = Accel()
''' Data for the Gate '''
gate_pose = Pose2D()
gate_vel = Pose2D()
gate_acc = Accel()
# Initialization Function
def __init__(self):
''' Subscriber '''
self.subJointStates = rospy.Subscriber("/joint_states", JointState, self.cbJointStates)
# tf message need to be treated differently
self.tf = tf.TransformListener()
# Timer: Update posX and posY for calculating velocity
rospy.Timer(rospy.Duration(1/(self.freq)), self.cbTimerVel, oneshot=False)
# TODO: More subscribers
# Publisher
self.pubTBCommand = rospy.Publisher("TBCommand", Pose2D, queue_size = 100)
'''Callback Functions'''
def cbJointStates(self, data):
self.angLeft = data.position[0]
self.angRight = data.position[1]
self.angAbs = (self.angLeft - self.angRight)%(2*180)
if self.angAbs > 180:
self.angAbs -= 2*180
# TODO: Radian to Degree
def cbTimerVel(self, event):
# update
self.posX[1] = self.tb_pos[0]; self.posY[1] = self.tb_pos[1]; self.posT[1] = self.angAbs
# calculate
self.tb_vel.x = (self.posX[1] - self.posX[0])/(1/self.freq)
self.tb_vel.y = (self.posY[1] - self.posY[0])/(1/self.freq)
self.tb_vel.theta = (self.posT[1] - self.posT[0])/(1/self.freq)
self.posX[0] = self.posX[1]; self.posY[0] = self.posY[1]; self.posT[0] = self.posT[1]
'''Signal Processing Functions'''
def _run(self):
while not rospy.is_shutdown():
'''
self.tb_cmd.x = 0.0
self.tb_cmd.y = 0.0
self.tb_cmd.theta = 0.0
'''
if self.tf.frameExists("/odom") and self.tf.frameExists("/base_footprint"):
t = self.tf.getLatestCommonTime("/odom", "/base_footprint")
self.tb_pos = self.tf.lookupTransform("/odom", "/base_footprint", t)[0]
self.pubTBCommand.publish(self.tb_vel)
def aplicarComandoJuntas(self,data): #Variavel que receber o comando a ser enviado para as juntas comandoPub=Accel() # theta_1 = -180 (-3.14) a 180 (3.14) # theta_2 = -90 (-1.57) a 150 (2.62) # theta_3 = -180 (-3.14) a 75 (1.30) # theta_4 = -400 (-6.98) a 400 (6.98) # theta_5 = -125 (-2.18) a 120 (2.1) # theta_6 = -400 (-6.98) a 400 (6.98) #Montando a variavel que sera publicada # Theta 1 if data[0] < -3.14: comandoPub.linear.x = -3.14 elif data[0] > 3.14: comandoPub.linear.x = 3.14 else: comandoPub.linear.x = data[0] # Theta 2 if data[1] < -1.57: comandoPub.linear.y = -1.57 elif data[1] > 2.62: comandoPub.linear.y = 2.62 else: comandoPub.linear.y = data[1] # Theta 3 if data[2] < -3.14: comandoPub.linear.z = -3.14 elif data[2] > 1.30: comandoPub.linear.z = 1.30 else: comandoPub.linear.z = data[2] # Theta 4 if data[3] < -6.98: comandoPub.angular.x = -6.98 elif data[3] > 6.98: comandoPub.angular.x = 6.98 else: comandoPub.angular.x = data[3] # Theta 5 if data[4] < -2.18: comandoPub.angular.y = -2.18 elif data[4] > 2.1: comandoPub.angular.y = 2.1 else: comandoPub.angular.y = data[4] # Theta 6 if data[5] < -6.98: comandoPub.angular.z = -6.98 elif data[5] > 6.98: comandoPub.angular.z = 6.98 else: comandoPub.angular.z = data[5] #Publicando o comando a ser enviado self.pub.publish(comandoPub)
def update(self, timer_event):
t = rospy.Time.now()
self.add_pt(ResponseType.time, t.to_sec() - self.startup_time)
# Do Odom data:
# position
pose = self.last_odom.pose.pose
try:
zyx = stf.Rotation.from_quat(tl(pose.orientation)).as_euler("ZYX")
except:
zyx = [0, 0, 0]
self.add_pt(ResponseType.pos_x, pose.position.x)
self.add_pt(ResponseType.pos_y, pose.position.y)
self.add_pt(ResponseType.pos_z, pose.position.z)
self.add_pt(ResponseType.pos_wx, zyx[2])
self.add_pt(ResponseType.pos_wy, zyx[1])
self.add_pt(ResponseType.pos_wz, zyx[0])
# velocity
twist = self.last_odom.twist.twist
self.add_pt(ResponseType.vel_x, twist.linear.x)
self.add_pt(ResponseType.vel_y, twist.linear.y)
self.add_pt(ResponseType.vel_z, twist.linear.z)
self.add_pt(ResponseType.vel_wx, twist.angular.x)
self.add_pt(ResponseType.vel_wy, twist.angular.y)
self.add_pt(ResponseType.vel_wz, twist.angular.z)
# acceleration
if self.last_vel is None:
# Prevents acceleration spike at the beginning
self.last_vel = twist
accel = Accel()
accel.linear.x = (twist.linear.x - self.last_vel.linear.x) / self.dt
accel.linear.y = (twist.linear.y - self.last_vel.linear.y) / self.dt
accel.linear.z = (twist.linear.z - self.last_vel.linear.z) / self.dt
accel.angular.x = (twist.angular.x - self.last_vel.angular.x) / self.dt
accel.angular.y = (twist.angular.y - self.last_vel.angular.y) / self.dt
accel.angular.z = (twist.angular.z - self.last_vel.angular.z) / self.dt
self.last_vel = twist
# low pass filter acceleration:
if self.last_accel is None:
# Prevents slow ramp of initial accel measurement
self.last_accel = accel
alph = float(self.accel_alpha)
accel.linear.x = self.last_accel.linear.x * alph + accel.linear.x * (
1.0 - alph)
accel.linear.y = self.last_accel.linear.y * alph + accel.linear.y * (
1.0 - alph)
accel.linear.z = self.last_accel.linear.z * alph + accel.linear.z * (
1.0 - alph)
accel.angular.x = self.last_accel.angular.x * alph + accel.angular.x * (
1.0 - alph)
accel.angular.y = self.last_accel.angular.y * alph + accel.angular.y * (
1.0 - alph)
accel.angular.z = self.last_accel.angular.z * alph + accel.angular.z * (
1.0 - alph)
self.last_accel = accel
self.add_pt(ResponseType.acc_x, accel.linear.x)
self.add_pt(ResponseType.acc_y, accel.linear.y)
self.add_pt(ResponseType.acc_z, accel.linear.z)
self.add_pt(ResponseType.acc_wx, accel.angular.x)
self.add_pt(ResponseType.acc_wy, accel.angular.y)
self.add_pt(ResponseType.acc_wz, accel.angular.z)
# Do cmd_pos
pose = self.last_cmd_pos.pose
try:
zyx = stf.Rotation.from_quat(tl(pose.orientation)).as_euler("ZYX")
except:
zyx = [0, 0, 0]
self.add_pt(ResponseType.cmd_pos_x, pose.position.x)
self.add_pt(ResponseType.cmd_pos_y, pose.position.y)
self.add_pt(ResponseType.cmd_pos_z, pose.position.z)
self.add_pt(ResponseType.cmd_pos_wx, zyx[2])
self.add_pt(ResponseType.cmd_pos_wy, zyx[1])
self.add_pt(ResponseType.cmd_pos_wz, zyx[0])
# Do cmd_vel
twist = self.last_cmd_vel
self.add_pt(ResponseType.cmd_vel_x, twist.linear.x)
self.add_pt(ResponseType.cmd_vel_y, twist.linear.y)
self.add_pt(ResponseType.cmd_vel_z, twist.linear.z)
self.add_pt(ResponseType.cmd_vel_wx, twist.angular.x)
self.add_pt(ResponseType.cmd_vel_wy, twist.angular.y)
self.add_pt(ResponseType.cmd_vel_wz, twist.angular.z)
# Do cmd_acc
accel = self.last_cmd_acc
self.add_pt(ResponseType.cmd_acc_x, accel.linear.x)
self.add_pt(ResponseType.cmd_acc_y, accel.linear.y)
self.add_pt(ResponseType.cmd_acc_z, accel.linear.z)
self.add_pt(ResponseType.cmd_acc_wx, accel.angular.x)
self.add_pt(ResponseType.cmd_acc_wy, accel.angular.y)
self.add_pt(ResponseType.cmd_acc_wz, accel.angular.z)
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
])
roll = (r * rad2degress) - 180
pitch = (p * rad2degress)
yaw = (y * rad2degress)
rospy.init_node("pid_control_node")
pub = rospy.Publisher('cmd_accel', Accel, queue_size=1)
rospy.Subscriber('odom', Odometry, odomCallback)
srv = Server(pidConfig,
reconfig_callback) # define dynamic_reconfigure callback
rate = rospy.Rate(30) # 30hz
cmdMsg = Accel()
rospy.loginfo("Starting up pid controller...")
while not rospy.is_shutdown():
cP = pidP(getBoundedAngleError(pitch - P_d))
cR = pidR(getBoundedAngleError(roll - R_d))
cY = pidY(getBoundedAngleError(yaw - Y_d))
cD = pidD(depth)
cV_x = pidVx(v_x)
cmdMsg.linear.x = cV_x
cmdMsg.linear.z = cD
cmdMsg.angular.x = cP
cmdMsg.angular.y = cR
def updateActors(self):
"""update actor dynamic info
"""
# pose = self.scenario_xml.ego_pose
# twist_linear = self.ego_vehicle.get_velocity()
# twist_angular = self.ego_vehicle.get_angular_velocity()
# self.ego_pose = Pose(
# position=Point(
# x=pose.location.x,
# y=pose.location.y,
# z=pose.location.z
# ),
# orientation=self.rotation_to_quaternion(self.scenario_xml.ego_pose.rotation)
# )
#
# self.ego_twist = Twist(
# linear=Vector3(
# x=twist_linear.x,
# y=twist_linear.y,
# z=twist_linear.z
# ),
# angular=Vector3(
# x=twist_angular.x,
# y=twist_angular.y,
# z=twist_angular.z
# )
# )
for i, actor in enumerate(self.carla_actors):
transform = actor.get_transform()
if transform.location == carla.Vector3D(0.0,0.0,0.0):
print('ros_bridge: actor may be died. id : ', actor.id)
del self.ros_actors[i]
del self.carla_actors[i]
continue
accel = actor.get_acceleration()
twist_angular = actor.get_angular_velocity()
twist_linear = actor.get_velocity()
ros_actor = self.ros_actors[i]
ros_actor.header = Header(stamp=rospy.Time.now(), frame_id='map')
ros_actor.pose = Pose(
position=Point(
x=transform.location.x,
y=-transform.location.y,
z=transform.location.z
),
orientation=self.rotation_to_quaternion(transform.rotation)
)
# static object tend to sink under ground
# if ros_actor.classification == Object.CLASSIFICATION_UNKNOWN:
# ros_actor.pose.position.z += ros_actor.shape.dimensions[2] * 0.5
ros_actor.twist = Twist(
linear=Vector3(
x=twist_linear.x,
y=-twist_linear.y,
z=-twist_linear.z
),
angular=Vector3(
x=twist_angular.x,
y=-twist_angular.y,
z=-twist_angular.z
)
)
ros_actor.accel = Accel(
linear=Vector3(
x=accel.x,
y=-accel.y,
z=-accel.z
),
angular=Vector3()
)
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
# license removed for brevity
import rospy
from geometry_msgs.msg import Twist, Accel
from nav_msgs.msg import Odometry
from math import atan2
#全局变量声明
flowfield_velocity_latest = Twist() #记录最后一个到来的流场数据
control_msg_latest = Accel() #记录最后一个到来的控制输入
expect_velocity = Twist() #记录当前的静场运动速度
ground_truth_latest = Odometry() #记录当前的实时状态
flowfield_direction_publisher = rospy.Publisher('flowfield_direction', Twist, queue_size=10)
def OnFlowfieldMsg(flowfield_velocity):
#全局变量声明
global flowfield_velocity_latest
global flowfield_direction_publisher
flowfield_velocity_latest = flowfield_velocity;
flowfield_direction = Twist();
flowfield_speed_square = (flowfield_velocity.linear.x * flowfield_velocity.linear.x + flowfield_velocity.linear.y * flowfield_velocity.linear.y)
flowfield_direction.linear.x=flowfield_velocity.linear.x / flowfield_speed_square
flowfield_direction.linear.y=flowfield_velocity.linear.y / flowfield_speed_square
flowfield_direction_publisher.publish(flowfield_direction)
def OnControlMsg(control_msg):
#全局变量声明
global control_msg_latest
control_msg_latest = control_msg
def _parse_keyboard(self):
# Save key peing pressed
key_press = self._get_key()
# Set Vehicle Speed #
if key_press == "1":
self.speed = 1
if key_press == "2":
self.speed = 2
# Choose ros message accordingly
if self._msg_type == 'twist':
cmd = Twist()
else:
cmd = Accel()
# If a key is pressed assign relevent linear / angular vel
if key_press != '':
# Linear velocities:
# Forward
if key_press == "w":
self.l.x = self._speed_windup(self.l.x, self.linear_increment,
self.linear_limit, False)
# Backwards
if key_press == "s":
self.l.x = self._speed_windup(self.l.x, self.linear_increment,
self.linear_limit, True)
# Left
if key_press == "a":
self.l.y = self._speed_windup(self.l.y, self.linear_increment,
self.linear_limit, False)
# Right
if key_press == "d":
self.l.y = self._speed_windup(self.l.y, self.linear_increment,
self.linear_limit, True)
# Up
if key_press == "x":
self.l.z = self._speed_windup(self.l.z, self.linear_increment,
self.linear_limit * 0.5, False)
# Down
if key_press == "z":
self.l.z = self._speed_windup(self.l.z, self.linear_increment,
self.linear_limit * 0.5, True)
# Angular Velocities
# Roll Left
if key_press == "j":
self.a.x = self._speed_windup(self.a.x, self.linear_increment,
self.linear_limit, True)
# Roll Right
if key_press == "l":
self.a.x = self._speed_windup(self.a.x, self.linear_increment,
self.linear_limit, False)
# Pitch Down
if key_press == "i":
self.a.y = self._speed_windup(self.a.y, self.linear_increment,
self.linear_limit, False)
# Pitch Up
if key_press == "k":
self.a.y = self._speed_windup(self.a.y, self.linear_increment,
self.linear_limit, True)
# Yaw Left
if key_press == "q":
self.a.z = self._speed_windup(self.a.z, self.linear_increment,
self.linear_limit, False)
# Yaw Right
if key_press == "e":
self.a.z = self._speed_windup(self.a.z, self.linear_increment,
self.linear_limit, True)
else:
# If no button is pressed reset velocities to 0
self.l = Vector3(x=0, y=0, z=0)
self.a = Vector3(x=0, y=0, z=0)
# Store velocity message into Twist format
cmd.angular = self.a
cmd.linear = self.l
# If ctrl+c kill node
if (key_press == '\x03'):
self.get_logger().info('Keyboard Interrupt Pressed')
self.get_logger().info('Shutting down [%s] node' % node_name)
# Set twists to 0
cmd.angular = Vector3(x=0, y=0, z=0)
cmd.linear = Vector3(x=0, y=0, z=0)
self._output_pub.publish(cmd)
exit(-1)
# Publish message
self._output_pub.publish(cmd)
def callback(data):
global published
if published:
return
print('Generating trajectory...')
# generate trajectory
max_height = rospy.get_param('~max_height')
distance = rospy.get_param('~forward_dist')
velocity = rospy.get_param('~velocity')
acceleration = rospy.get_param('~acceleration')
frequency = rospy.get_param('~frequency')
# publish to /trajectory
orientation_q = data.pose.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
origin = data.pose.pose.position
up = gen_traj(max_height, acceleration, velocity, frequency)
forward = gen_traj(distance, acceleration, velocity, frequency)
points = []
seq = 0
for pt in up:
point = Point(origin.x, origin.y, origin.z + pt.x)
linear_v = Vector3(0, 0, pt.y)
angular_v = Vector3(0, 0, 0)
vel = Twist(linear_v, angular_v)
linear_a = Vector3(0, 0, pt.z)
angular_a = Vector3(0, 0, 0)
acc = Accel(linear_a, angular_a)
stamp = rospy.Time.now()
header = Header(seq, stamp, '/gentraj')
seq += 1
point = PVAYStamped(header, point, yaw, vel, acc)
points.append(point)
for pt in forward:
point = Point(origin.x + math.cos(yaw) * pt.x,
origin.y + math.sin(yaw) * pt.x, origin.z + max_height)
linear_v = Vector3(pt.y, 0, 0)
angular_v = Vector3(0, 0, 0)
vel = Twist(linear_v, angular_v)
linear_a = Vector3(pt.z, 0, 0)
angular_a = Vector3(0, 0, 0)
acc = Accel(linear_a, angular_a)
stamp = rospy.Time.now()
header = Header(seq, stamp, '/gentraj')
seq += 1
point = PVAYStamped(header, point, yaw, vel, acc)
points.append(point)
for pt in up[::-1]:
point = Point(origin.x + math.cos(yaw) * distance,
origin.y + math.sin(yaw) * distance, origin.z + pt.x)
linear_v = Vector3(0, 0, -pt.y)
angular_v = Vector3(0, 0, 0)
vel = Twist(linear_v, angular_v)
linear_a = Vector3(0, 0, -pt.z)
angular_a = Vector3(0, 0, 0)
acc = Accel(linear_a, angular_a)
stamp = rospy.Time.now()
header = Header(seq, stamp, '/gentraj')
seq += 1
point = PVAYStamped(header, point, yaw, vel, acc)
points.append(point)
# posx = [v.pos.x for v in points]
# posy = [v.pos.y for v in points]
# posz = [v.pos.z for v in points]
# fig = plt.figure()
# ax = Axes3D(fig)
# ax.scatter(posx, posy, posz)
# plt.show()
# publish to /trajectory
pub = rospy.Publisher('/trajectory', PVAYStampedTrajectory, queue_size=5)
traj = PVAYStampedTrajectory(points)
rospy.sleep(7)
pub.publish(traj)
print('published!')
published = True
def set_aa_vel(self, vel):
msg = Accel()
msg.linear.x = vel[0]
msg.linear.y = vel[1]
self.set_aabox_vel.publish(msg)
def __init__(self, usb_port='/dev/ttyACM0'):
self.ser = serial.Serial( # port='/dev/cu.usbmodem1422',
port='/dev/ttyACM0',
baudrate=2000000)
self.a1 = Accel()
self.a2 = Accel()