|
@ -8,11 +8,11 @@ import os |
|
|
import csv |
|
|
import csv |
|
|
|
|
|
|
|
|
# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to |
|
|
# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to |
|
|
# control the Stretch robot. |
|
|
|
|
|
|
|
|
# control the Stretch robot |
|
|
from control_msgs.msg import FollowJointTrajectoryGoal |
|
|
from control_msgs.msg import FollowJointTrajectoryGoal |
|
|
|
|
|
|
|
|
# Import JointTrajectoryPoint from the trajectory_msgs package to define |
|
|
# Import JointTrajectoryPoint from the trajectory_msgs package to define |
|
|
# robot trajectories. |
|
|
|
|
|
|
|
|
# robot trajectories |
|
|
from trajectory_msgs.msg import JointTrajectoryPoint |
|
|
from trajectory_msgs.msg import JointTrajectoryPoint |
|
|
|
|
|
|
|
|
# We're going to subscribe to 64-bit integers, so we need to import the definition |
|
|
# We're going to subscribe to 64-bit integers, so we need to import the definition |
|
@ -31,7 +31,7 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
""" |
|
|
""" |
|
|
def __init__(self, export_data=False): |
|
|
def __init__(self, export_data=False): |
|
|
""" |
|
|
""" |
|
|
Function that initializes the subscriber class="p">,and other features. |
|
|
|
|
|
|
|
|
Function that initializes the subscriber and other features. |
|
|
:param self: The self reference. |
|
|
:param self: The self reference. |
|
|
:param export_data: A boolean message type. |
|
|
:param export_data: A boolean message type. |
|
|
""" |
|
|
""" |
|
@ -53,7 +53,6 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
# Create boolean data type for conditional statements if you want to export effort data. |
|
|
# Create boolean data type for conditional statements if you want to export effort data. |
|
|
self.export_data = export_data |
|
|
self.export_data = export_data |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def callback(self, msg): |
|
|
def callback(self, msg): |
|
|
""" |
|
|
""" |
|
|
Callback function to update and store JointState messages. |
|
|
Callback function to update and store JointState messages. |
|
@ -63,7 +62,6 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
# Store the joint messages for later use |
|
|
# Store the joint messages for later use |
|
|
self.joint_states = msg |
|
|
self.joint_states = msg |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def issue_command(self): |
|
|
def issue_command(self): |
|
|
""" |
|
|
""" |
|
|
Function that makes an action call and sends joint trajectory goals |
|
|
Function that makes an action call and sends joint trajectory goals |
|
@ -76,11 +74,11 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
trajectory_goal = FollowJointTrajectoryGoal() |
|
|
trajectory_goal = FollowJointTrajectoryGoal() |
|
|
trajectory_goal.trajectory.joint_names = self.joints |
|
|
trajectory_goal.trajectory.joint_names = self.joints |
|
|
|
|
|
|
|
|
# Provide desired positions for joint name. |
|
|
|
|
|
|
|
|
# Provide desired positionsfor joint name. |
|
|
point0 = JointTrajectoryPoint() |
|
|
point0 = JointTrajectoryPoint() |
|
|
point0.positions = [0.9] |
|
|
point0.positions = [0.9] |
|
|
|
|
|
|
|
|
# Set a list to the trajectory_goal.trajectory.points |
|
|
|
|
|
|
|
|
# Set a list to the `trajectory_goal.trajectory.points` |
|
|
trajectory_goal.trajectory.points = [point0] |
|
|
trajectory_goal.trajectory.points = [point0] |
|
|
|
|
|
|
|
|
# Specify the coordinate frame that we want (base_link) and set the time to be now. |
|
|
# Specify the coordinate frame that we want (base_link) and set the time to be now. |
|
@ -93,7 +91,6 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
rospy.loginfo('Sent position goal = {0}'.format(trajectory_goal)) |
|
|
rospy.loginfo('Sent position goal = {0}'.format(trajectory_goal)) |
|
|
self.trajectory_client.wait_for_result() |
|
|
self.trajectory_client.wait_for_result() |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def feedback_callback(self,feedback): |
|
|
def feedback_callback(self,feedback): |
|
|
""" |
|
|
""" |
|
|
The feedback_callback function deals with the incoming feedback messages |
|
|
The feedback_callback function deals with the incoming feedback messages |
|
@ -130,7 +127,6 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
else: |
|
|
else: |
|
|
self.joint_effort.append(current_effort) |
|
|
self.joint_effort.append(current_effort) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def done_callback(self, status, result): |
|
|
def done_callback(self, status, result): |
|
|
""" |
|
|
""" |
|
|
The done_callback function will be called when the joint action is complete. |
|
|
The done_callback function will be called when the joint action is complete. |
|
@ -157,7 +153,6 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
writer.writerow(self.joints) |
|
|
writer.writerow(self.joints) |
|
|
writer.writerows(self.joint_effort) |
|
|
writer.writerows(self.joint_effort) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main(self): |
|
|
def main(self): |
|
|
""" |
|
|
""" |
|
|
Function that initiates the issue_command function. |
|
|
Function that initiates the issue_command function. |
|
@ -170,7 +165,6 @@ class JointActuatorEffortSensor(hm.HelloNode): |
|
|
self.issue_command() |
|
|
self.issue_command() |
|
|
time.sleep(2) |
|
|
time.sleep(2) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__': |
|
|
if __name__ == '__main__': |
|
|
try: |
|
|
try: |
|
|
# Declare object, node, from JointActuatorEffortSensor class |
|
|
# Declare object, node, from JointActuatorEffortSensor class |
|
|