def goto_frame(self):
'''
Plays one single frame
'''
self.set_all_joints_stiff()
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k,v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
for k, v in self._checkBoxesPower[self._widget.frameList.currentItem().text()].items():
torque_msg.joint_names.append(k)
torque_msg.on.append(v)
self.effort_pub.publish(torque_msg)
self._joint_pub.publish(pos_msg)
def cb(msg):
msg2 = JointCommand()
msg2.joint_names = msg.position.joint_names
msg2.positions = msg.position.points[0].positions
msg2.velocities = [-1] * len(msg2.joint_names)
msg2.accelerations = [-1] * len(msg2.joint_names)
msg2.max_currents = [-1] * len(msg2.joint_names)
pub.publish(msg2)
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LShoulderPitch", "RShoulderPitch", 'LShoulderRoll',
'RShoulderRoll'
]
joint_command_msg.positions = [1.57, 1.57, 0.3, -0.3]
return joint_command_msg
def animation_callback(self, msg):
""" The animation server is sending us goal positions for the next keyframe"""
self.blackboard.last_animation_goal_time = msg.header.stamp.to_sec()
if msg.request:
rospy.loginfo(
"Got Animation request. HCM will try to get controllable now.")
# animation has to wait
# state machine should try to become controllable
self.blackboard.animation_requested = True
return
if msg.first:
if msg.hcm:
# comming from ourselves
# we don't have to do anything, since we must be in the right state
pass
else:
# comming from outside
# check if we can run an animation now
if self.blackboard.current_state != STATE_CONTROLABLE:
rospy.logwarn(
"HCM is not controllable, animation refused.")
return
else:
# we're already controllable, go to animation running
self.blackboard.external_animation_running = True
if msg.last:
if msg.hcm:
# This was an animation from the DSD
self.blackboard.hcm_animation_finished = True
pass
else:
# this is the last frame, we want to tell the DSD, that we're finished with the animations
self.blackboard.external_animation_running = False
if msg.position is None:
# probably this was just to tell us we're finished
# we don't need to set another position to the motors
return
# forward positions to motors, if some where transmitted
if len(msg.position.points) > 0:
out_msg = JointCommand()
out_msg.positions = msg.position.points[0].positions
out_msg.joint_names = msg.position.joint_names
out_msg.accelerations = [-1] * len(out_msg.joint_names)
out_msg.velocities = [-1] * len(out_msg.joint_names)
out_msg.max_currents = [-1] * len(out_msg.joint_names)
if self.blackboard.shut_down_request:
# there are sometimes transmittions errors during shutdown due to race conditions
# there is nothing we can do so just ignore the errors in this case
try:
self.joint_goal_publisher.publish(out_msg)
except:
pass
else:
self.joint_goal_publisher.publish(out_msg)
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LElbow", "RElbow", "LShoulderPitch", "RShoulderPitch"
]
joint_command_msg.positions = [
math.radians(-60),
math.radians(60),
math.radians(120.0),
math.radians(-120.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LElbow", "RElbow", "LShoulderPitch", "RShoulderPitch"
]
joint_command_msg.positions = [
math.radians(35.86),
math.radians(-36.10),
math.radians(75.27),
math.radians(-75.58)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"left_arm_motor_0 [shoulder]", "right_arm_motor_0 [shoulder]",
"left_arm_motor_1", "right_arm_motor_1"
]
joint_command_msg.positions = [
math.radians(0),
math.radians(0),
math.radians(170),
math.radians(170)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LeftElbow", "RightElbow", "LeftShoulderPitch [shoulder]",
"RightShoulderPitch [shoulder]"
]
joint_command_msg.positions = [
math.radians(-90.0),
math.radians(90.0),
math.radians(45.0),
math.radians(-45.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"Shoulder-L [shoulder]", "Shoulder-R [shoulder]", "UpperArm-L",
"UpperArm-R", "LowerArm-L", "LowerArm-R"
]
joint_command_msg.positions = [
math.radians(60.0),
math.radians(-60.0),
math.radians(10.0),
math.radians(-10.0),
math.radians(-135.0),
math.radians(135.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"l_el", "r_el", "l_sho_pitch", "r_sho_pitch", "l_sho_roll",
"r_sho_roll"
]
joint_command_msg.positions = [
math.radians(-140),
math.radians(140),
math.radians(-135),
math.radians(135),
math.radians(-90),
math.radians(90)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"left_shoulder_pitch [shoulder]",
"right_shoulder_pitch [shoulder]", "left_shoulder_roll",
"right_shoulder_roll", "left_elbow", "right_elbow"
]
joint_command_msg.positions = [
math.radians(60.0),
math.radians(60.0),
math.radians(10.0),
math.radians(10.0),
math.radians(-135.0),
math.radians(-135.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"leftElbowYaw", "rightElbowYaw", "leftShoulderPitch[shoulder]",
"rightShoulderPitch[shoulder]", "leftShoulderYaw",
"rightShoulderYaw"
]
joint_command_msg.positions = [
math.radians(-160),
math.radians(160),
math.radians(75.27),
math.radians(75.58),
math.radians(-75.58),
math.radians(75.58)
]
return joint_command_msg
def box_ticked(self):
'''
Controls whether a checkbox has been clicked, and reacts.
'''
for k, v in self._treeItems.items():
self._motorSwitched[k] = (v.checkState(0) == 2)
for k, v in self._motorSwitched.items():
self._textFields[k].setEnabled(v)
self._sliders[k].setEnabled(v)
self.set_sliders_and_text_fields(manual=False)
## Wolfgang part
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
if self._widget.treeModeSelector.currentIndex() == 0:
for k, v in sorted(self._treeItems.items()):
torque_msg.joint_names.append(k)
torque_msg.on.append(v.checkState(0) == 2)
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k, v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
self._joint_pub.publish(pos_msg)
self.effort_pub.publish(torque_msg)
if not self._widget.frameList.currentItem() == None:
if not self._widget.frameList.currentItem().text(
) == "#CURRENT_FRAME":
self.treeModeChanged(
self._widget.treeModeSelector.currentIndex())
self.record(keep=True)
def goto_init(self):
'''
Plays init frame, sets all motor values to 0
'''
self.set_all_joints_stiff()
if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
for k, v in self._workingValues.iteritems():
self._workingValues[k] = 0.0
for k, v in self._currentGoals.iteritems():
self._currentGoals[k] = 0.0
self.set_sliders_and_text_fields(manual=False)
pos_msg = JointCommand()
pos_msg.joint_names = self._initial_joints.name
pos_msg.velocities = [1.0] * len(self._initial_joints.name)
pos_msg.positions = [0.0] * len(self._initial_joints.name)
pos_msg.accelerations = [-1.0] * len(self._initial_joints.name)
pos_msg.max_currents = [-1.0] * len(self._initial_joints.name)
self._joint_pub.publish(pos_msg)
"LHipRoll": 9,
"LHipPitch": 10,
"LKnee": 11,
"LAnklePitch": 12,
"LAnkleRoll": 13,
"RHipYaw": 14,
"RHipRoll": 15,
"RHipPitch": 16,
"RKnee": 17,
"RAnklePitch": 18,
"RAnkleRoll": 19
}
rospy.init_node("send_joint_command")
pos_msg = JointCommand()
pos_msg.joint_names = ids.keys()
#pos_msg.joint_names = ["RShoulderRoll"]
pos_msg.velocities = [-1.0] * 20
pos_msg.positions = [0.0] * 20
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
pub = rospy.Publisher("/DynamixelController/command",
JointCommand,
queue_size=1)
print(pos_msg)
while not rospy.is_shutdown():
pos_msg.header.stamp = rospy.Time.now()
pub.publish(pos_msg)
rospy.sleep(0.1)
rospy.init_node('test_look_at')
head_tf_frame = "base_link"
tf_buffer = tf2.Buffer(rospy.Duration(5))
tf_listener = tf2.TransformListener(tf_buffer)
request = IKRequest()
request.group_name = "Head"
request.timeout.secs = 1
request.approximate = True
request.look_at_goals.append(LookAtGoal())
request.look_at_goals[0].link_name = "camera"
request.look_at_goals[0].weight = 1
request.look_at_goals[0].axis.x = 1
pos_msg = JointCommand()
pos_msg.joint_names = ["HeadPan", "HeadTilt"]
pos_msg.positions = [0, 0]
pos_msg.velocities = [1.5, 1.5]
pos_msg.accelerations = [-1, -1]
pos_msg.max_currents = [-1, -1]
rospy.wait_for_service("/bio_ik/get_bio_ik")
bio_ik = rospy.ServiceProxy('/bio_ik/get_bio_ik', GetIK)
publish_motor_goals = rospy.Publisher('/head_motor_goals',
JointCommand,
queue_size=10)
while not rospy.is_shutdown():
x = float(input('x: '))
y = float(input('y: '))
def reset(self):
# completly reset pybullet, since it is strange
if self.sim_type == "pybullet":
pass # self.sim.reset_simulation()
# reset Dynup. send emtpy message to just cancel all goals
self.dynup_cancel_pub.publish(GoalID())
if self.direction == "front":
self.hand_ground_time = self.dynup_params["time_hands_side"] + \
self.dynup_params["time_hands_rotate"] + \
self.dynup_params["time_foot_close"] + \
self.dynup_params["time_hands_front"] + \
self.dynup_params["time_foot_ground_front"] + \
self.dynup_params["time_torso_45"]
self.rise_phase_time = self.hand_ground_time + \
self.dynup_params["time_to_squat"]
self.in_squat_time = self.rise_phase_time + \
self.dynup_params["wait_in_squat_front"]
self.total_trial_length = self.in_squat_time + \
self.dynup_params["rise_time"]
print(self.total_trial_length)
elif self.direction == "back":
self.hand_ground_time = self.dynup_params["time_legs_close"] + \
self.dynup_params["time_foot_ground_back"]
self.rise_phase_time = self.hand_ground_time + \
self.dynup_params["time_full_squat_hands"] + \
self.dynup_params["time_full_squat_legs"]
self.in_squat_time = self.rise_phase_time + \
self.dynup_params["wait_in_squat_back"]
self.total_trial_length = self.in_squat_time + \
self.dynup_params["rise_time"]
else:
print(f"Direction {self.direction} not known")
# trial continues for 3 sec after dynup completes
if self.real_robot:
input("Please hold robot in the air and then press any key")
else:
# reset simulation
self.sim.set_gravity(False)
self.sim.set_self_collision(False)
self.sim.reset_robot_pose((0, 0, 1), (0, 0, 0, 1))
time = self.get_time()
while self.get_time() - time < 10:
msg = JointCommand()
if self.robot == "wolfgang":
msg.joint_names = [
"HeadPan", "HeadTilt", "LElbow", "LShoulderPitch",
"LShoulderRoll", "RElbow", "RShoulderPitch",
"RShoulderRoll", "LHipYaw", "LHipRoll", "LHipPitch",
"LKnee", "LAnklePitch", "LAnkleRoll", "RHipYaw",
"RHipRoll", "RHipPitch", "RKnee", "RAnklePitch",
"RAnkleRoll"
]
if self.direction == "back":
msg.positions = [
0, 0, 0.82, 0.89, 0, -0.82, -0.89, 0, -0.01, 0.06,
0.47, 1.01, -0.45, 0.06, 0.01, -0.06, -0.47, -1.01,
0.45, -0.06
] # walkready
elif self.direction == "front":
msg.positions = [
0, 0.78, 0.78, 1.36, 0, -0.78, -1.36, 0, 0.11, 0.07,
-0.19, 0.23, -0.63, 0.07, 0.11, -0.07, 0.19, -0.23,
0.63, -0.07
] # falling_front
elif self.robot == "robotis_op2":
msg.joint_names = [
"head_pan", "head_tilt", "LElbow", "l_sho_pitch",
"l_sho_roll", "RElbow", "r_sho_pitch", "r_sho_roll",
"LHipYaw", "l_hip_roll", "l_hip_pitch", "l_knee",
"l_ank_pitch", "l_ank_roll", "r_hip_yaw", "r_hip_roll",
"r_hip_pitch", "r_knee", "r_ank_pitch", "RAnkleRoll"
]
if self.direction == "back":
msg.positions = [
0, 0, 0.79, 0, 0, -0.79, 0, 0, -0.01, 0.06, 0.47, 1.01,
-0.45, 0.06, 0.01, -0.06, -0.47, -1.01, 0.45, -0.06
] # walkready
elif self.direction == "front":
msg.positions = [
0, 0.78, 0.78, 1.36, 0, -0.78, -1.36, 0, 0.11, 0.07,
-0.19, 0.23, -0.63, 0.07, 0.11, -0.07, 0.19, -0.23,
0.63, -0.07
] # falling_front
elif self.robot == "sigmaban":
msg.joint_names = [
"head_yaw", "head_pitch", "LElbow", "left_shoulder_pitch",
"left_shoulder_roll", "RElbow", "right_shoulder_pitch",
"right_shoulder_roll", "left_hip_yaw", "left_hip_roll",
"left_hip_pitch", "left_knee", "left_ankle_pitch",
"left_ankle_roll", "right_hip_yaw", "right_hip_roll",
"right_hip_pitch", "right_knee", "right_ankle_pitch",
"right_ankle_roll"
]
if self.direction == "back":
msg.positions = [
0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0, 0.0, 0
] # walkready
elif self.direction == "front":
msg.positions = [
0, 0.0, 0.0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0, 0.0,
0.0, 0, 0, 0, 0, 0
] # falling_front
self.dynamixel_controller_pub.publish(msg)
if not self.real_robot:
self.sim.step_sim()
if self.real_robot:
input("Please put the robot on the ground and then press any key")
else:
self.reset_position()
self.sim.set_gravity(True)
self.sim.set_self_collision(True)
time = self.get_time()
while not self.get_time() - time > 2:
self.sim.step_sim()
# Head Part
rospy.Subscriber("joint_states", JointState, joint_state_cb, queue_size=1)
if rospy.get_param("sim_active", default=False):
head_pub = rospy.Publisher("head_motor_goals",
JointCommand,
queue_size=1)
else:
head_pub = rospy.Publisher("DynamixelController/command",
JointCommand,
queue_size=1)
head_msg = JointCommand()
head_msg.max_currents = [-1] * 2
head_msg.velocities = [5] * 2
head_msg.accelerations = [40] * 2
head_msg.joint_names = ['HeadPan', 'HeadTilt']
head_msg.positions = [0] * 2
head_pan_step = 0.05
head_tilt_step = 0.05
try:
print(msg)
while (1):
key = getKey()
if key in moveBindings.keys():
x += moveBindings[key][0] * x_speed_step
x = round(x, 2)
y += moveBindings[key][1] * y_speed_step
y = round(y, 2)
def __init__(self,
ros_active=False,
robot='wolfgang',
do_ros_init=True,
robot_node=None,
base_ns='',
recognize=False,
camera_active=True,
foot_sensors_active=True):
"""
The RobotController, a Webots controller that controls a single robot.
The environment variable WEBOTS_ROBOT_NAME should be set to "amy", "rory", "jack" or "donna" if used with
4_bots.wbt or to "amy" if used with 1_bot.wbt.
:param ros_active: Whether ROS messages should be published
:param robot: The name of the robot to use, currently one of wolfgang, darwin, nao, op3
:param do_ros_init: Whether to call rospy.init_node (only used when ros_active is True)
:param external_controller: Whether an external controller is used, necessary for RobotSupervisorController
:param base_ns: The namespace of this node, can normally be left empty
"""
self.ros_active = ros_active
self.recognize = recognize
self.camera_active = camera_active
self.foot_sensors_active = foot_sensors_active
if robot_node is None:
self.robot_node = Robot()
else:
self.robot_node = robot_node
self.walkready = [0] * 20
self.time = 0
self.motors = []
self.sensors = []
# for direct access
self.motors_dict = {}
self.sensors_dict = {}
self.timestep = int(self.robot_node.getBasicTimeStep())
self.switch_coordinate_system = True
self.is_wolfgang = False
self.pressure_sensors = None
if robot == 'wolfgang':
self.is_wolfgang = True
self.proto_motor_names = [
"RShoulderPitch [shoulder]", "LShoulderPitch [shoulder]",
"RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow",
"RHipYaw", "LHipYaw", "RHipRoll [hip]", "LHipRoll [hip]",
"RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan",
"HeadTilt"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.initial_joint_positions = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 75,
"LShoulderRoll": 0,
"LElbow": 36,
"RShoulderPitch": -75,
"RShoulderRoll": 0,
"RElbow": -36,
"HeadPan": 0,
"HeadTilt": 0
}
self.sensor_suffix = "_sensor"
accel_name = "imu accelerometer"
gyro_name = "imu gyro"
camera_name = "camera"
self.pressure_sensor_names = []
if self.foot_sensors_active:
self.pressure_sensor_names = [
"llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"
]
self.pressure_sensors = []
for name in self.pressure_sensor_names:
sensor = self.robot_node.getDevice(name)
sensor.enable(self.timestep)
self.pressure_sensors.append(sensor)
elif robot == 'darwin':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
elif robot == 'nao':
self.proto_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch",
"LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch",
"LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw",
"HeadPitch"
]
self.external_motor_names = self.proto_motor_names
self.sensor_suffix = "S"
accel_name = "accelerometer"
gyro_name = "gyro"
camera_name = "CameraTop"
self.switch_coordinate_system = False
elif robot == 'op3':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll",
"r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll",
"l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee",
"r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll",
"head_pan", "head_tilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
self.switch_coordinate_system = False
self.motor_names_to_external_names = {}
self.external_motor_names_to_motor_names = {}
for i in range(len(self.proto_motor_names)):
self.motor_names_to_external_names[
self.proto_motor_names[i]] = self.external_motor_names[i]
self.external_motor_names_to_motor_names[
self.external_motor_names[i]] = self.proto_motor_names[i]
self.current_positions = {}
self.joint_limits = {}
for motor_name in self.proto_motor_names:
motor = self.robot_node.getDevice(motor_name)
motor.enableTorqueFeedback(self.timestep)
self.motors.append(motor)
self.motors_dict[
self.motor_names_to_external_names[motor_name]] = motor
sensor = self.robot_node.getDevice(motor_name + self.sensor_suffix)
sensor.enable(self.timestep)
self.sensors.append(sensor)
self.sensors_dict[
self.motor_names_to_external_names[motor_name]] = sensor
self.current_positions[self.motor_names_to_external_names[
motor_name]] = sensor.getValue()
# min, max and middle position (precomputed since it will be used at each step)
self.joint_limits[
self.motor_names_to_external_names[motor_name]] = (
motor.getMinPosition(), motor.getMaxPosition(),
0.5 * (motor.getMinPosition() + motor.getMaxPosition()))
self.accel = self.robot_node.getDevice(accel_name)
self.accel.enable(self.timestep)
self.gyro = self.robot_node.getDevice(gyro_name)
self.gyro.enable(self.timestep)
if self.is_wolfgang:
self.accel_head = self.robot_node.getDevice(
"imu_head accelerometer")
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera_counter = 0
if self.camera_active:
self.camera.enable(self.timestep * CAMERA_DIVIDER)
if self.recognize:
self.camera.recognitionEnable(self.timestep)
self.last_img_saved = 0.0
self.img_save_dir = "/tmp/webots/images" + \
time.strftime("%Y-%m-%d-%H-%M-%S") + \
os.getenv('WEBOTS_ROBOT_NAME')
if not os.path.exists(self.img_save_dir):
os.makedirs(self.img_save_dir)
self.imu_frame = rospy.get_param("~imu_frame", "imu_frame")
if self.ros_active:
if base_ns == "":
clock_topic = "/clock"
else:
clock_topic = base_ns + "clock"
if do_ros_init:
rospy.init_node("webots_ros_interface",
argv=['clock:=' + clock_topic])
self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole")
self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole")
self.camera_optical_frame = rospy.get_param(
"~camera_optical_frame", "camera_optical_frame")
self.head_imu_frame = rospy.get_param("~head_imu_frame",
"imu_frame_2")
self.pub_js = rospy.Publisher(base_ns + "joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher(base_ns + "imu/data_raw",
Imu,
queue_size=1)
self.pub_imu_head = rospy.Publisher(base_ns + "imu_head/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher(base_ns + "camera/image_proc",
Image,
queue_size=1)
self.pub_cam_info = rospy.Publisher(base_ns + "camera/camera_info",
CameraInfo,
queue_size=1,
latch=True)
self.pub_pres_left = rospy.Publisher(base_ns +
"foot_pressure_left/raw",
FootPressure,
queue_size=1)
self.pub_pres_right = rospy.Publisher(base_ns +
"foot_pressure_right/raw",
FootPressure,
queue_size=1)
self.cop_l_pub_ = rospy.Publisher(base_ns + "cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher(base_ns + "cop_r",
PointStamped,
queue_size=1)
rospy.Subscriber(base_ns + "DynamixelController/command",
JointCommand, self.command_cb)
# publish camera info once, it will be latched
self.cam_info = CameraInfo()
self.cam_info.header.stamp = rospy.Time.from_seconds(self.time)
self.cam_info.header.frame_id = self.camera_optical_frame
self.cam_info.height = self.camera.getHeight()
self.cam_info.width = self.camera.getWidth()
f_y = self.mat_from_fov_and_resolution(
self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height,
self.cam_info.width), self.cam_info.height)
f_x = self.mat_from_fov_and_resolution(self.camera.getFov(),
self.cam_info.width)
self.cam_info.K = [
f_x, 0, self.cam_info.width / 2, 0, f_y,
self.cam_info.height / 2, 0, 0, 1
]
self.cam_info.P = [
f_x, 0, self.cam_info.width / 2, 0, 0, f_y,
self.cam_info.height / 2, 0, 0, 0, 1, 0
]
self.pub_cam_info.publish(self.cam_info)
if robot == "op3":
# start pose
command = JointCommand()
command.joint_names = ["r_sho_roll", "l_sho_roll"]
command.positions = [-math.tau / 8, math.tau / 8]
self.command_cb(command)
# needed to run this one time to initialize current position, otherwise velocity will be nan
self.get_joint_values(self.external_motor_names)