Skip to content

Instantly share code, notes, and snippets.

@hello-binit
Created May 2, 2024 03:42
Show Gist options
  • Save hello-binit/4eee80b0c3842aa2e55eba9b535e5ce3 to your computer and use it in GitHub Desktop.
Save hello-binit/4eee80b0c3842aa2e55eba9b535e5ce3 to your computer and use it in GitHub Desktop.
#!/usr/bin/env python3
import rclpy
from action_msgs.msg import GoalStatus
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectoryPoint
import sys
import time
class ManipulationTest(Node):
def __init__(self):
super().__init__('manipulation_test')
self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory')
if not self.trajectory_client.wait_for_server(timeout_sec=10.0):
self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.')
sys.exit()
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
else:
self.get_logger().info('Goal failed with status: {0}'.format(status))
def move_to_configuration(self):
trajectory_goal = FollowJointTrajectory.Goal()
goal_time_tolerance = rclpy.duration.Duration(seconds=1.0)
trajectory_goal.goal_time_tolerance = goal_time_tolerance.to_msg()
trajectory_goal.trajectory.joint_names = ['translate_mobile_base']
point = JointTrajectoryPoint()
point.time_from_start = rclpy.duration.Duration(seconds=2.0).to_msg()
point.positions = [0.0]
trajectory_goal.trajectory.joint_names = trajectory_goal.trajectory.joint_names
trajectory_goal.trajectory.points = [point]
trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
self._send_goal_future = self.trajectory_client.send_goal_async(trajectory_goal)
self._send_goal_future.add_done_callback(self.goal_response_callback)
if __name__ == '__main__':
rclpy.init(args=None)
manipulation_test = ManipulationTest()
manipulation_test.move_to_configuration()
rclpy.spin(manipulation_test)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment