def joystick_callback(data):
global sphere_head_axis0
global sphere_head_axis1
global sphere_head_axis2
global joystick_control_active
global emotion
global movement_left
global movement_right
global head_tracking_speed
axis0 = data.axes[1] / 6.0
axis1 = data.axes[0] / 3.0
axis2 = data.axes[2] * 1.5
if abs(axis0) > 0.00001 or abs(axis1) > 0.00001 or abs(axis2) > 0.00001:
joystick_control_active = True
sphere_head_axis0.publish(axis0)
sphere_head_axis1.publish(axis1)
sphere_head_axis2.publish(axis2)
rospy.loginfo("%f %f %f" % (axis0, axis1, axis2))
else:
joystick_control_active = False
if (data.buttons[6]):
emotion.publish('shy')
print('shy')
elif (data.buttons[7]):
emotion.publish('kiss')
print('kiss')
elif (data.buttons[8]):
emotion.publish('hearts')
print('hearts')
elif (data.buttons[9]):
emotion.publish('smileblink')
print('smileblink')
elif (data.buttons[10]):
emotion.publish('teeth')
print('teeth')
elif (data.buttons[0]):
msg = PerformMovementsActionGoal()
msg.goal.actions = ['shoulder_left_hug']
movement_left.publish(msg)
msg.goal.actions = ['shoulder_right_hug']
movement_right.publish(msg)
print("HUG HUG HUG")
elif (data.axes[4] == 1):
emotion.publish('lookleft')
elif (data.axes[4] == -1):
emotion.publish('lookright')
head_tracking_speed = 4 - (data.axes[3] + 1) * 2
x = 0.5 + data.axes[0]
y = -0.5 - data.axes[1]
z = 0 + 0.5 * (1 + data.axes[3])
pos = InteractiveMarkerFeedback()
pos.header.frame_id = 'world'
pos.marker_name = "arm_left_palm"
pos.event_type = 5
pos.pose.position.x = x
pos.pose.position.y = y
pos.pose.position.z = z
rospy.loginfo_throttle(
1, "%f %f %f" %
(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z))
vis = Marker()
vis.type = 10
vis.id = 69696969
vis.header.frame_id = 'world'
vis.pose = pos.pose
vis.mesh_resource = "package://robots/common/meshes/visuals/target.stl"
vis.scale.x = 0.01
vis.scale.y = 0.01
vis.scale.z = 0.01
vis.color.r = 0
vis.color.g = 1
vis.color.b = 1
vis.color.a = 1
visualisation.publish(vis)
interactive_marker.publish(pos)
MARKER_MANAGER.register_marker("length1" , 9 , "base_link", (255,255,255), 0.2)
MARKER_MANAGER.register_marker("v2", 9 , "base_link", (255,255,255), 0.2)
MARKER_MANAGER.register_marker("w2" , 9 , "base_link", (255,255,255), 0.2)
MARKER_MANAGER.register_marker("r2", 9 , "base_link", (255,255,255), 0.2)
MARKER_MANAGER.register_marker("theta_p2" , 9 , "base_link", (255,255,255), 0.2)
MARKER_MANAGER.register_marker("length2" , 9 , "base_link", (255,255,255), 0.2)
#---- init lines ------# Yellow line for arcs
MARKER_MANAGER.register_marker("r_start", 4 , "base_link", (255,255,0), 0.02)
MARKER_MANAGER.register_marker("r_end" , 4 , "base_link", (255,255,0), 0.02)
# Center of rotation
MARKER_MANAGER.register_marker("arc_center_start", 2 ,"base_link", (255,255,255), 0.2)
MARKER_MANAGER.register_marker("arc_center_end", 2 ,"base_link", (255,255,255), 0.2)
#---- Init Markers ----#
init = InteractiveMarkerFeedback()
init.marker_name = ""
init.header.frame_id = "base_link"
car_1.car1_feedback_cb(init)
car_2.car2_feedback_cb(init)
r = rospy.Rate(30) #call at 30HZ
while (not rospy.is_shutdown()):
cal_double_arc((car_1.x, car_1.y, car_1.theta), (car_2.x, car_2.y, car_2.theta))
print (str(CAR_MID_XYT))
rc1 = car_1.run_once()
rc2 = car_2.run_once()
if rc1 or rc2:
#---- update car_1 ----#
pub_car_1_result.publish(car_1.kinematic_result)
#---- update car_2 -----#
pub_car_2_result.publish(car_2.kinematic_result)
def main(args):
global server
#----- Init node ------#
rospy.init_node('kinematics', anonymous=True)
#----- Publisher -------#
pub_car_1_result = rospy.Publisher('car_1_result',
PoseStamped,
queue_size=10,
latch=True)
pub_car_2_result = rospy.Publisher('car_2_result',
PoseStamped,
queue_size=10,
latch=True)
pub_car_big_result = rospy.Publisher('car_big_result',
PoseStamped,
queue_size=10,
latch=True)
pub_car_1_current = rospy.Publisher('car_1_current',
PoseStamped,
queue_size=10,
latch=True)
pub_car_2_current = rospy.Publisher('car_2_current',
PoseStamped,
queue_size=10,
latch=True)
pub_car_big_current = rospy.Publisher('car_big_current',
PoseStamped,
queue_size=10,
latch=True)
#-- publish marker --#
pub_marker_sphere = rospy.Publisher('marker_sphere',
MarkerArray,
queue_size=1,
latch=True)
pub_marker_line = rospy.Publisher('marker_line',
MarkerArray,
queue_size=1,
latch=True)
pub_marker_text = rospy.Publisher('marker_text',
MarkerArray,
queue_size=1,
latch=True)
#------- Interactive Markers ---------#
server = InteractiveMarkerServer("basic_controls")
#------- Markers: Two cars -----------#
# print ("car_1 theta : " + str(car_1.theta))
q = tf.transformations.quaternion_from_euler(0, 0, car_1.theta)
pose = Pose(Point(car_1.x, car_1.y, 0), Quaternion(q[0], q[1], q[2],
q[3])) # 0 degree
make6DofMarker(False, InteractiveMarkerControl.MOVE_ROTATE_3D, pose, True,
"car_1")
q = tf.transformations.quaternion_from_euler(0, 0, car_2.theta)
pose = Pose(Point(car_2.x, car_2.y, 0), Quaternion(q[0], q[1], q[2],
q[3])) # 45 degree
make6DofMarker(False, InteractiveMarkerControl.MOVE_ROTATE_3D, pose, True,
"car_2")
#------- Markers: Two sliders -----------#
position = Point(-3, INIT_VC / SLIDER_GAIN, 0)
makeYaxisMarker(position, "Vc")
position = Point(-3.5, INIT_WC / SLIDER_GAIN, 0)
makeYaxisMarker(position, "Wc")
# --- init text markers ---#
set_text((-3, INIT_VC + 1), str(round(car_big.v, 2)), (255, 255, 255), 0.3,
0) # Vc slider text
set_text((-3.5, INIT_VC + 1), str(round(car_big.w, 2)), (255, 255, 255),
0.3, 1) # Wc slider text
set_text((0, -1), "V_car_1 : " + str(round(car_1.v, 2)), (255, 255, 255),
0.3, 2)
set_text((0, -1.5), "W_car_1 : " + str(round(car_1.w, 2)), (255, 255, 255),
0.3, 3)
set_text((0, -2), "V_car_2 : " + str(round(car_2.v, 2)), (255, 255, 255),
0.3, 4)
set_text((0, -2.5), "W_car_2 : " + str(round(car_2.w, 2)), (255, 255, 255),
0.3, 5)
# set_text((-3,0) , "Theta_car_1 : " + str(round(car_1.theta ,2)) , (255,255,255) , 0.3, 6)
# set_text((-3,-0.5) , "Theta_P_car_1 : " + str(round(car_1.theta_p ,2)) , (255,255,255) , 0.3, 7)
# set_text((-3,-1) , "Theta_car_2 : " + str(round(car_2.theta ,2)) , (255,255,255) , 0.3, 8)
# set_text((-3,-1.5) , "Theta_P_car_2 : " + str(round(car_2.theta_p ,2)) , (255,255,255) , 0.3, 9)
#---- init lines ------# Yellow line for arcs
set_line([], (255, 255, 0), 0.02, 0) # car_1
set_line([], (255, 255, 0), 0.02, 1) # car_2
set_line([], (255, 255, 0), 0.02, 2) # car_big
p = cal_small_car_position((car_big.x, car_big.y, car_big.theta))
set_line([p[0], p[1]], (150, 0, 0), 0.1, 3) # REd line
p = cal_small_car_position((car_big.x_p, car_big.y_p, car_big.theta_p))
set_line([p[0], p[1]], (255, 0, 0), 0.1, 4) # Red line
#---- init spheres -----#
set_sphere((car_1.x_c, car_1.y_c), (0, 255, 255), 0.2, 0)
set_sphere((car_2.x_c, car_2.y_c), (0, 255, 255), 0.2, 1)
set_sphere((car_big.x_c, car_big.y_c), (0, 255, 255), 0.2, 2)
#---- Init Markers ----#
init = InteractiveMarkerFeedback()
init.marker_name = ""
init.header.frame_id = "base_link"
marker_feedback_CB(init)
r = rospy.Rate(30) #call at 30HZ
while (not rospy.is_shutdown()):
if car_1.is_need_pub or car_2.is_need_pub or car_big.is_need_pub:
#---- update car big pose ----#
modify_line(
cal_small_car_position((car_big.x, car_big.y, car_big.theta)),
3)
modify_line(
cal_small_car_position(
(car_big.x_p, car_big.y_p, car_big.theta_p)), 4)
#---- update car_1 ----#
pub_car_1_result.publish(car_1.kinematic_result)
# print (car_1.kinematic_current)
pub_car_1_current.publish(car_1.kinematic_current)
#---- update car_2 -----#
pub_car_2_result.publish(car_2.kinematic_result)
pub_car_2_current.publish(car_2.kinematic_current)
#----- update car_big ----#
pub_car_big_result.publish(car_big.kinematic_result)
pub_car_big_current.publish(car_big.kinematic_current)
#---- update Textes -----#
pub_marker_text.publish(marker_text)
#---- update center of rotations -----#
pub_marker_sphere.publish(marker_sphere)
#---- update lines -----#
pub_marker_line.publish(marker_line)
#--- Reset flags ------#
car_1.is_need_pub = False
car_2.is_need_pub = False
car_big.is_need_pub = False
r.sleep()