thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Added comments

Browse source 
ar3_config launch file
cobot_platform move.py
joystick move.py
mgi cpp file
This commit is contained in:
John Farrell 2022-03-15 21:06:20 -04:00
parent 3b21854d5e
commit 2d858ab961
Signed by: john
GPG key ID: 53EF5A5459F750F5
4 changed files with 61 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
src/ar3_config/launch/dev.launch.py
View file

@ -6,6 +6,7 @@ from launch_ros.descriptions import ComposableNode
from launch.actions import ExecuteProcess
import yaml
# Helper functions to make passing parameters less disgusting
def get_file(path):
with open(path, 'r') as f:
return f.read()
@ -16,16 +17,18 @@ def get_yaml(path):
def generate_launch_description():
# Get package locations
launch_root = get_package_share_directory('ar3_config')
description_root = get_package_share_directory('ar3_description')
config_root = join(launch_root, 'config')
# Load files for the urdf(robot model), srdf (semantic model), and the kinematics
robot_description = {'robot_description': get_file(join(description_root, 'ar3.urdf'))}
robot_description_semantic = {'robot_description_semantic': get_file(join(description_root, 'ar3.srdf'))}
robot_description_kinematics = get_yaml(join(config_root, 'kinematics.yaml'))
return LaunchDescription([
# RViz
# Start RViz (ui)
Node(
package='rviz2',
executable='rviz2',
@ -42,6 +45,7 @@ def generate_launch_description():
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
),
# creates the cpp interface so you can use /move_pose
Node(
package='move_group_interface',
executable='move_group_interface_node',
@ -53,13 +57,14 @@ def generate_launch_description():
get_yaml(join(config_root, "ar3_controllers.yaml"))],
),
# Our joystick node
Node(
package='joystickmove',
executable='move',
output='screen',
),
# Create joystick controller
# Create joystick controller and ComposableNodes for servo_server
ComposableNodeContainer(
name="joystick_container",
namespace="/",
@ -88,7 +93,7 @@ def generate_launch_description():
output="screen",
),
# Controller
# Setup the controller manager
Node(
package='controller_manager',
executable='ros2_control_node',
@ -106,7 +111,7 @@ def generate_launch_description():
shell=True,
output='screen'),
# Run platform
# Run platform for interface between teensy
Node(
package='cobot_platform',
executable='move',
@ -114,6 +119,7 @@ def generate_launch_description():
),
# Start the actual move_group node/action server
# TODO: AAAAAAAAAAAA
Node(
package="moveit_ros_move_group",
executable="move_group",

15
src/cobot_platform/cobot_platform/move.py
View file

@ -12,17 +12,21 @@ from serial import Serial
class CobotMove(Node):
def __init__(self):
super().__init__('cobot_move')
# Set up a logger for the node
self._logger = self.get_logger()
self._logger.info('COBOT init')
# Connect to the serial port on the teensy
self._serial = Serial('/dev/ttyACM0', 115200)
# Set initial joint values to all 0s
self._cur = [0.0] * 6
# create a timer to update all the joint angles at once
self._timer = self.create_timer(1 / 40, self._dosomething)
# Set joint angles independantly
base_topic = 'move/'
self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data)
@ -32,6 +36,7 @@ class CobotMove(Node):
qos = QoSProfile(depth=10)
# Publishing the values so other nodes can read them (so they don't need to keep track of current positions)
publish_base_topic = 'update/'
self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', qos)
self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', qos)
@ -40,9 +45,11 @@ class CobotMove(Node):
self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', qos)
self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', qos)
# Set the initial position of the robot to 0
self._serial.write(b'STA0.0.1\n')
print(self._serial.readline())
# Callbacks for move/jX subscriptions
def _move_j1_cb(self, msg):
self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
@ -61,11 +68,14 @@ class CobotMove(Node):
def _move_j6_cb(self, msg):
self._cur[5] = int(msg.data / pi / 2. * 2000)
# Send all current positions to the teensy
def _dosomething(self):
self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
# Get a response from line above of positions
line = self._serial.readline().decode('UTF-8')
# Parse the line into each angle
blocks = re.findall(r'[ABCDEF]-*\d+', line)
a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
b = float(blocks[1][1:]) / 10000 / 4. * pi * 2.
@ -74,6 +84,7 @@ class CobotMove(Node):
e = float(blocks[4][1:]) / 2000. * pi * 2.
f = float(blocks[5][1:]) / 2000. * pi * 2.
# Publish those to the update/jX subscribers
self.s_j1.publish(Float32(data=a))
self.s_j2.publish(Float32(data=b))
self.s_j3.publish(Float32(data=c))
@ -85,9 +96,11 @@ class CobotMove(Node):
def main(args=None):
rclpy.init(args=args)
# start the node
cobot_move = CobotMove()
rclpy.spin(cobot_move)
# kill the node
cobot_move.destroy_node()
rclpy.shutdown()

31
src/joystickmove/joystickmove/move.py
View file

@ -11,6 +11,8 @@ from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
from enum import Enum
# Mappings for a DualShock4 (PS4) controller
class Buttons(Enum):
CROSS = 0
CIRCLE = 1
@ -39,35 +41,48 @@ class Axes(Enum):
class JoyMove(Node):
def __init__(self):
super().__init__('joystickmove')
# Get the logger
self._logger = self.get_logger()
self._logger.info('joystick init')
# Set up topics for publishing
JOY_TOPIC = '/joy'
TWIST_TOPIC = '/servo_server/delta_twist_cmds'
JOINT_TOPIC = '/servo_server/delta_joint_cmds'
# Define some constants
EE = 'joint_6'
BASE = 'base_link'
# Create the needed subscribers and publishers for moveit_servo
self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
# Start a moveit_servo client
self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
# Check if the a service is available
while not self.servo_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('servo_server not available, waiting again...')
self._logger.info('servo_server connected.')
# Create a request to tell the server the joystick exists
req = Trigger.Request()
self.servo_client.call_async(req)
# Callback for any button or stick that is pressed on the joystick
def _joy_cb(self, msg):
axes = msg.axes
buttons = msg.buttons
if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]:
# If you press main buttons or dpad arrows
if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] \
or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]:
# combine default values and whatever you have pushed to create a JointJog object
joint = JointJog()
joint.joint_names = ['joint_1', 'joint_2',
@ -76,39 +91,51 @@ class JoyMove(Node):
float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
joint.duration = 0.1
# Add a timestamp so we know when the action actually took place
joint.header.stamp = self.get_clock().now().to_msg()
joint.header.frame_id = "link_3"
joint.header.frame_id = "link_6"
# Publish the JointJog to servo_server
self.joint_pub.publish(joint)
return
# If you pressed a trigger or stick
ts = TwistStamped()
# Define linear depth from right stick
ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
ts.twist.linear.y = axes[Axes.RIGHT_X.value]
# Define linear X from trigger analog values
lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
ts.twist.linear.x = lin_x_right + lin_x_left
# Define angular up and down from left stick
ts.twist.angular.y = axes[Axes.LEFT_Y.value]
ts.twist.angular.x = axes[Axes.LEFT_X.value]
# Left and Right bumpers to control roll
roll_positive = buttons[Buttons.R2.value]
roll_negative = -1 * buttons[Buttons.L2.value]
ts.twist.angular.z = float(roll_positive + roll_negative)
# Add a timestamp so we know when the action actually took place
ts.header.stamp = self.get_clock().now().to_msg()
ts.header.frame_id = "base_link"
# Publish the Twist to servo_server
self.twist_pub.publish(ts)
def main(args=None):
rclpy.init(args=args)
# Start the node
joy_move = JoyMove()
rclpy.spin(joy_move)
# Kill the node
joy_move.destroy_node()
rclpy.shutdown()

16
src/move_group_interface/src/move_group_interface_node.cpp
View file

@ -15,13 +15,9 @@ const std::string PLANNING_GROUP = "ar3_arm";
class ScanNPlan : public rclcpp::Node {
public:
ScanNPlan() : Node("scan_n_plan") {
// create the node and start it
// nh_ = std::make_shared<Node>(reinterpret_cast<Node *>(this));
//nh_ = shared_from_this();
RCLCPP_INFO(this->get_logger(), "SNP START");
// create subscriber
// Create /move_pose subscriber
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose",
rclcpp::QoS(1),
@ -34,11 +30,14 @@ private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
(void) p;
RCLCPP_INFO(this->get_logger(), "GOT MSG");
// Get a shared pointer to the MoveItCpp library to use our planning scene and getting states
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(shared_from_this());
RCLCPP_INFO(this->get_logger(), "GOT MOVEIT");
//auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
//psm->startSceneMonitor();
// ??????
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
@ -54,16 +53,17 @@ private:
RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR");
planning_components->setStartStateToCurrentState();
// set the goal to the request pose
// Set the goal to the request pose
RCLCPP_INFO(this->get_logger(), "SET GOAL");
planning_components->setGoal(*p, "base_link");
// actually plan
// Actually plan
RCLCPP_INFO(this->get_logger(), "DO PLAN");
auto plan_solution = planning_components->plan();
// plan is valid
// Plan is valid
if (plan_solution) {
// Follow through with the planned path
planning_components->execute();
}
}