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

Joystick almost works

Browse source
This commit is contained in:
John Farrell 2021-09-24 18:47:29 -04:00
parent 056fa3de68
commit 3db39b9ec3
Signed by: john
GPG key ID: 10543A0DA2EC1E12
6 changed files with 77 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/ar3_config/config/ar3_joy.yaml
View file

@ -34,7 +34,7 @@ planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'wo
## Other frames
ee_frame_name: link_6 # The name of the end effector link, used to return the EE pose
robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
## Stopping behaviour
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command

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

@ -1,7 +1,8 @@
from os.path import join
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.actions import ExecuteProcess
import yaml
@ -73,7 +74,7 @@ def generate_launch_description():
)
],
output="screen",
)
),
# Controller
Node(

14
src/ar3_hardware/include/ar3_hardware/ar3_system.hpp
View file

@ -60,6 +60,20 @@ private:
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j4_pub_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j5_pub_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j6_pub_;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j1;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j2;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j3;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j4;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j5;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j6;
void update_j1_cb(std_msgs::msg::Float32::SharedPtr msg);
void update_j2_cb(std_msgs::msg::Float32::SharedPtr msg);
void update_j3_cb(std_msgs::msg::Float32::SharedPtr msg);
void update_j4_cb(std_msgs::msg::Float32::SharedPtr msg);
void update_j5_cb(std_msgs::msg::Float32::SharedPtr msg);
void update_j6_cb(std_msgs::msg::Float32::SharedPtr msg);
};
} // namespace ar3_hardware

33
src/ar3_hardware/src/ar3_system.cpp
View file

@ -125,6 +125,13 @@ hardware_interface::return_type Ar3SystemHardware::start()
j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
s_j4 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j4", 10, std::bind(&Ar3SystemHardware::update_j4_cb, this, std::placeholders::_1));
s_j5 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j5", 10, std::bind(&Ar3SystemHardware::update_j5_cb, this, std::placeholders::_1));
s_j6 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j6", 10, std::bind(&Ar3SystemHardware::update_j6_cb, this, std::placeholders::_1));
status_ = hardware_interface::status::STARTED;
RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully started!");
@ -143,10 +150,35 @@ hardware_interface::return_type Ar3SystemHardware::stop()
return hardware_interface::return_type::OK;
}
void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
hw_positions_[0] = msg->data;
}
void Ar3SystemHardware::update_j2_cb(std_msgs::msg::Float32::SharedPtr msg) {
hw_positions_[1] = msg->data;
}
void Ar3SystemHardware::update_j3_cb(std_msgs::msg::Float32::SharedPtr msg) {
hw_positions_[2] = msg->data;
}
void Ar3SystemHardware::update_j4_cb(std_msgs::msg::Float32::SharedPtr msg) {
hw_positions_[3] = msg->data;
}
void Ar3SystemHardware::update_j5_cb(std_msgs::msg::Float32::SharedPtr msg) {
hw_positions_[4] = msg->data;
}
void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) {
hw_positions_[5] = msg->data;
}
hardware_interface::return_type Ar3SystemHardware::read()
{
//RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
/*
for (uint i = 0; i < hw_commands_.size(); i++)
{
hw_positions_[i] = 6.9;
@ -156,6 +188,7 @@ hardware_interface::return_type Ar3SystemHardware::read()
// "Got position state %.5f for '%s'!", hw_positions_[i],
// info_.joints[i].name.c_str());
}
*/
//RCLCPP_INFO(
// rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",

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

@ -38,6 +38,14 @@ class CobotMove(Node):
self.create_subscription(Float32, base_topic+'j5', self._move_j5_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
publish_base_topic = 'update/'
self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', rclpy.qos.qos_profile_sensor_data)
self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', rclpy.qos.qos_profile_sensor_data)
self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', rclpy.qos.qos_profile_sensor_data)
self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', rclpy.qos.qos_profile_sensor_data)
self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', rclpy.qos.qos_profile_sensor_data)
self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', rclpy.qos.qos_profile_sensor_data)
qos = QoSProfile(depth=10)
self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
self._joint_state = JointState()
@ -76,8 +84,8 @@ class CobotMove(Node):
line = self._serial.readline().decode('UTF-8')
blocks = re.findall(r'[ABCDEF]-*\d+', line)
a = float(blocks[0][1:]) / 2000. * pi * 2.
b = float(blocks[1][1:]) / 3840.64 * pi * 2.
a = float(blocks[0][1:]) / 8000. * pi * 2.
b = float(blocks[1][1:]) / 10000 * pi * 2.
c = float(blocks[2][1:]) / 10000. * pi * 2.
d = float(blocks[3][1:]) / 2000. * pi * 2.
e = float(blocks[4][1:]) / 2000. * pi * 2.
@ -89,8 +97,14 @@ class CobotMove(Node):
self._joint_pub.publish(self._joint_state)
# print(self._joint_state.position)
self.s_j1.publish(Float32(data=a))
self.s_j2.publish(Float32(data=b))
self.s_j3.publish(Float32(data=c))
self.s_j4.publish(Float32(data=d))
self.s_j5.publish(Float32(data=e))
self.s_j6.publish(Float32(data=f))
# self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
# def _push ?
def main(args=None):

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

@ -6,7 +6,7 @@ from std_srvs.srv import Trigger
from std_msgs.msg import Float32
from sensor_msgs.msg import Joy
from geometry_msgs.msg import TwistStamped
from moveit_msgs.msg import PlanningScene
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
from enum import Enum
@ -49,9 +49,8 @@ class JoyMove(Node):
self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
self.base_pub = self.create_publisher(Float32, JOY_TOPIC, 10)
self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
# self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
# Check if the a service is available
@ -62,6 +61,12 @@ class JoyMove(Node):
req = Trigger.Request()
self.servo_client.call_async(req)
'''
ps = PlanningScene
ps.world = PlanningSceneWorld
ps.is_diff = True
self.collision_pub.publish(ps)
'''
def _joy_cb(self, msg):
axes = msg.axes
@ -84,6 +89,7 @@ class JoyMove(Node):
ts.twist.angular.z = float(roll_positive + roll_negative)
ts.header.stamp = self.get_clock().now().to_msg()
ts.header.frame_id = "base_link"
self.twist_pub.publish(ts)