def set_current_pos(self, res_msg):
print(res_msg.data)
temp = res_msg.data
first = temp.find('[')
comma1 = temp.find(',')
comma2 = temp.find(',', comma1 + 1)
comma3 = temp.find(',', comma2 + 1)
end = temp.find(']')
"""# Build the current position message
self.current_pos = JointState()
self.current_pos.header.seq = 99999
self.current_pos.header.stamp = rospy.Time.now()
self.current_pos.header.frame_id = "/world"
self.current_pos.name = ["end_link", "joint0", "joint1", "joint3", "joint4"]
self.current_pos.position = [0.0,
int(temp[first+1:comma1]) * pi / 3140,
int(temp[comma1+1:comma2]) * pi / 3140,
int(temp[comma2+1:comma3]) * pi / 3140,
int(temp[comma3+1:end]) * pi / 3140]
print self.current_pos
# Publish the current position to the simulation
"""
angles = codesys_joint()
angles.theta0 = int(temp[first + 1:comma1]) * pi / 3140
angles.theta1 = int(temp[comma1 + 1:comma2]) * pi / 3140
angles.theta2 = int(temp[comma2 + 1:comma3]) * pi / 3140
angles.theta3 = int(temp[comma3 + 1:end]) * pi / 3140
self.go_to_joint_state(angles)
def straight_line(self):
hoekies = codesys_joint()
hoekies.theta0 = [
-1.5689095259, -1.5047105551, -1.4402370453, -1.3771352768,
-1.3112896681, -1.2422773838, -1.1736456156, -1.1029742956,
-1.0318784714, -0.95530653, -0.876789093
]
hoekies.theta1 = [
0.0037735873, 0.1321716607, 0.2611185312, 0.3873221874,
0.5190133452, 0.6570378542, 0.7943015099, 0.9356440306,
1.0778357983, 1.2309795618, 1.3880144358
]
hoekies.theta2 = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
hoekies.theta3 = [
-0.0037735873, -0.1321716607, -0.2611185312, -0.3873221874,
-0.5190133452, -0.6570378542, -0.7943015099, -0.9356440306,
-1.0778357983, -1.2309795618, -1.3880144358
]
for x in range(10, 11):
hoekje = smart_joint()
hoekje.theta0 = hoekies.theta0[x]
hoekje.theta1 = hoekies.theta1[x]
hoekje.theta2 = 0.0
hoekje.theta3 = hoekies.theta3[x]
hoekje.theta4 = 0.0
self.go_to_joint_state(hoekje)
def send_goal(self):
# Get information of the path
amount_of_waypoints = len(self.plan_of_wrist.joint_trajectory.points)
# Set variable to correct message type
servo_waypoints = codesys_joint()
# Set the amount of waypoints
servo_waypoints.waypoints = amount_of_waypoints
# Create empty lists to store the data
servo_waypoints.theta0 = [0] * amount_of_waypoints
servo_waypoints.theta1 = [0] * amount_of_waypoints
servo_waypoints.theta2 = [0] * amount_of_waypoints
servo_waypoints.theta3 = [0] * amount_of_waypoints
last_link_waypoint = 0.0
# Loop for storing the trajectory in servo angles used in codesys, every cycle stores one waypoint
for nb_of_waypoints in range(0, amount_of_waypoints):
# Store the joint values in temporary variables for clearity
# moveit_waypoint_theta0 = self.plan_of_wrist.joint_trajectory.points[nb_of_waypoints].positions[0]
# moveit_waypoint_theta1 = self.plan_of_wrist.joint_trajectory.points[nb_of_waypoints].positions[1]
moveit_waypoint_theta0 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[0]
moveit_waypoint_theta1 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[1]
moveit_waypoint_theta2 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[2]
# Translate the float64 points between -pi and pi to integer values used by the codesys
# and store them
servo_waypoints.theta0[nb_of_waypoints] = int(0)
servo_waypoints.theta1[nb_of_waypoints] = int(0)
servo_waypoints.theta2[nb_of_waypoints] = int(
(moveit_waypoint_theta0 / pi) * 3140)
servo_waypoints.theta3[nb_of_waypoints] = int(
(moveit_waypoint_theta1 / pi) * 3140)
# Pservo_waypoints to check if message was correct
print servo_waypoints
# Wait for the subscriber
# while (self.give_waypoints.get_num_connections() == 0):
# pass
if (amount_of_waypoints != 0):
# Publish list of waypoints to /codesys_waypoints
self.give_waypoints.publish(servo_waypoints)
rospy.loginfo("Waypoints sent to the server")
# Get last link last angle and publish it to the dynamixel
last_link_waypoint = self.plan_of_wrist.joint_trajectory.points[
amount_of_waypoints - 1].positions[2]
self.send_dynamixel.publish(last_link_waypoint)
rospy.loginfo(
"Goal sent to dynamixel: {}".format(last_link_waypoint))
else:
rospy.logwarn("O waypoints, no path available")
def send_goal(self):
# Get information of the path
self.amount_of_waypoints = len(
self.plan_of_wrist.joint_trajectory.points)
print(self.amount_of_waypoints)
# Set variable to correct message type
self.servo_waypoints = codesys_joint()
# Set the amount of waypoints
self.servo_waypoints.waypoints = self.amount_of_waypoints
# Create empty lists to store the data
self.servo_waypoints.theta0 = [0] * self.amount_of_waypoints
self.servo_waypoints.theta1 = [0] * self.amount_of_waypoints
self.servo_waypoints.theta2 = [0] * self.amount_of_waypoints
self.servo_waypoints.theta3 = [0] * self.amount_of_waypoints
# Loop for storing the trajectory in servo angles used in codesys, every cycle stores one waypoint
for nb_of_waypoints in range(0, self.amount_of_waypoints):
# Store the joint values in temporary variables for clearity
self.moveit_waypoint_theta0 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[0]
self.moveit_waypoint_theta1 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[1]
self.moveit_waypoint_theta2 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[2]
self.moveit_waypoint_theta3 = self.plan_of_wrist.joint_trajectory.points[
nb_of_waypoints].positions[3]
# Translate the float64 points between -pi and pi to integer values used by the codesys
# and store them
self.servo_waypoints.theta0[nb_of_waypoints] = int(
(self.moveit_waypoint_theta0 / pi) * 3140)
self.servo_waypoints.theta1[nb_of_waypoints] = int(
(self.moveit_waypoint_theta1 / pi) * 3140)
self.servo_waypoints.theta2[nb_of_waypoints] = int(
(self.moveit_waypoint_theta2 / pi) * 3140)
self.servo_waypoints.theta3[nb_of_waypoints] = int(
(self.moveit_waypoint_theta3 / pi) * 3140)
# Print servo_waypoints to check if message was correct
rospy.loginfo("Waiting ")
while (self.give_waypoints.get_num_connections() == 0):
pass
self.give_waypoints.publish(self.servo_waypoints)
rospy.loginfo("message send")
def send_goal(self):
# Get information of the path
print " "
print(self.plan_of_wrist.joint_trajectory.points[0])
self.servo_waypoints = codesys_joint()