def odom_cb(msg):
global odom_x, odom_y, model_x, model_y, first_call, prev_x, prev_y, prev_theta
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
ori = msg.pose.pose.orientation
theta = q2e([ori.x, ori.y, ori.z, ori.w])[2]
if first_call:
first_call = False
prev_x = x
prev_y = y
prev_theta = theta
model_x.append(x)
model_y.append(y)
else:
#for model
new_x, new_y, new_theta = update(prev_x, prev_y, prev_theta, 0.5,
-0.25)
model_x.append(new_x)
model_y.append(new_y)
prev_x = new_x
prev_y = new_y
prev_theta = new_theta
odom_x.append(x)
odom_y.append(y)
def pose_to_matrix(pose):
"""takes ros pose message and converts to 4x4 homogenous matrix"""
pos = [pose.position.x, pose.position.y, pose.position.z]
quat = pose.orientation
rpy = list(q2e((quat.x, quat.y, quat.z, quat.w)))
mat = transform_matrix(*pos + rpy)
return mat
def callback(self, data):
self.trans = np.array(
[data.pose.pose.position.x, data.pose.pose.position.z])
self.xy = np.array(
[data.pose.pose.position.x, data.pose.pose.position.y])
orientation_q = data.pose.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
self.ori = np.array([
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
])
rotation = np.array(q2e(orientation_list))
self.rot = rotation[1]
self.yaw = rotation[2]
self.roll = rotation[0]
def odom_cb(msg):
global state_x, state_y, transformed_x, transformed_y, init_set
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
ori = msg.pose.pose.orientation
theta = q2e([ori.x, ori.y, ori.z, ori.w])[2]
if not init_set:
init_set = True
ini_x = x;
ini_y = y;
ini_theta = theta
else:
tx, ty, ttht = transform_pose(x,y,theta)
transformed_x.append(tx)
transformed_y.append(ty)
state_x.append(x)
state_y.append(y)
def get_obs(self):
self.unpause_sim()
obs = []
states = self.get_relative_state()
self.pause_sim()
for neighbour, state in states.items():
translation = np.array(state[0])[:-1]
rotation = np.array(q2e(state[1])[-1]) # extract only YAW
# print (self.goal)
obs.append(
np.hstack([
translation, rotation, self.goal[neighbour],
self.previous_act
]))
obs = self.scale_obs(np.hstack(obs))
self.previous_obs = deepcopy(obs)
obs = {
"observation": self.previous_obs,
"achieved_goal": self.previous_obs[self.parent.goal_indices],
"desired_goal": np.hstack(self.goal.values())
}
return obs
from sensor_msgs.msg import JointState
from tf import TransformBroadcaster
from tf.transformations import quaternion_from_euler as q2e
from math import sin, cos, pi
joint_state = JointState()
odom_broadcaster = TransformBroadcaster()
angle = 0
if __name__ == "__main__":
rospy.init_node("controller")
robot_control = rospy.Publisher('/joint_states', JointState, queue_size=10)
rate = rospy.Rate(30)
while not rospy.is_shutdown():
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
'base_to_wheel1', 'base_to_wheel2', 'base_to_wheel3',
'base_to_wheel4'
]
joint_state.position = [0, 0, 0, 0, 0]
x, y, z = cos(angle), sin(angle), 0
odom_quat = q2e(0, 0, angle)
print('%d', '%d', x, y)
robot_control.publish(joint_state)
odom_broadcaster.sendTransform((x, y, z), odom_quat, rospy.Time.now(),
'base_link', 'odom')
rate.sleep()
def tf_preprocessor(x):
trans, rot = x
return np.array(list(trans[:2]) + list(q2e(rot)[-1:]))