Joystick almost works
This commit is contained in:
parent
056fa3de68
commit
3db39b9ec3
6 changed files with 77 additions and 9 deletions
|
@ -34,7 +34,7 @@ planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'wo
|
||||||
|
|
||||||
## Other frames
|
## Other frames
|
||||||
ee_frame_name: link_6 # The name of the end effector link, used to return the EE pose
|
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
|
## Stopping behaviour
|
||||||
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command
|
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command
|
||||||
|
|
|
@ -1,7 +1,8 @@
|
||||||
from os.path import join
|
from os.path import join
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
from launch import LaunchDescription
|
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
|
from launch.actions import ExecuteProcess
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
|
@ -73,7 +74,7 @@ def generate_launch_description():
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
output="screen",
|
output="screen",
|
||||||
)
|
),
|
||||||
|
|
||||||
# Controller
|
# Controller
|
||||||
Node(
|
Node(
|
||||||
|
|
|
@ -60,6 +60,20 @@ private:
|
||||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j4_pub_;
|
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 j5_pub_;
|
||||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j6_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
|
} // namespace ar3_hardware
|
||||||
|
|
|
@ -125,6 +125,13 @@ hardware_interface::return_type Ar3SystemHardware::start()
|
||||||
j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
|
j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
|
||||||
j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 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;
|
status_ = hardware_interface::status::STARTED;
|
||||||
|
|
||||||
RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully 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;
|
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()
|
hardware_interface::return_type Ar3SystemHardware::read()
|
||||||
{
|
{
|
||||||
//RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
|
//RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
|
||||||
|
|
||||||
|
/*
|
||||||
for (uint i = 0; i < hw_commands_.size(); i++)
|
for (uint i = 0; i < hw_commands_.size(); i++)
|
||||||
{
|
{
|
||||||
hw_positions_[i] = 6.9;
|
hw_positions_[i] = 6.9;
|
||||||
|
@ -156,6 +188,7 @@ hardware_interface::return_type Ar3SystemHardware::read()
|
||||||
// "Got position state %.5f for '%s'!", hw_positions_[i],
|
// "Got position state %.5f for '%s'!", hw_positions_[i],
|
||||||
// info_.joints[i].name.c_str());
|
// info_.joints[i].name.c_str());
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
//RCLCPP_INFO(
|
//RCLCPP_INFO(
|
||||||
// rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",
|
// rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",
|
||||||
|
|
|
@ -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+'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)
|
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)
|
qos = QoSProfile(depth=10)
|
||||||
self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
|
self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
|
||||||
self._joint_state = JointState()
|
self._joint_state = JointState()
|
||||||
|
@ -76,8 +84,8 @@ class CobotMove(Node):
|
||||||
line = self._serial.readline().decode('UTF-8')
|
line = self._serial.readline().decode('UTF-8')
|
||||||
|
|
||||||
blocks = re.findall(r'[ABCDEF]-*\d+', line)
|
blocks = re.findall(r'[ABCDEF]-*\d+', line)
|
||||||
a = float(blocks[0][1:]) / 2000. * pi * 2.
|
a = float(blocks[0][1:]) / 8000. * pi * 2.
|
||||||
b = float(blocks[1][1:]) / 3840.64 * pi * 2.
|
b = float(blocks[1][1:]) / 10000 * pi * 2.
|
||||||
c = float(blocks[2][1:]) / 10000. * pi * 2.
|
c = float(blocks[2][1:]) / 10000. * pi * 2.
|
||||||
d = float(blocks[3][1:]) / 2000. * pi * 2.
|
d = float(blocks[3][1:]) / 2000. * pi * 2.
|
||||||
e = float(blocks[4][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)
|
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 _push ?
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
|
|
|
@ -6,7 +6,7 @@ from std_srvs.srv import Trigger
|
||||||
from std_msgs.msg import Float32
|
from std_msgs.msg import Float32
|
||||||
from sensor_msgs.msg import Joy
|
from sensor_msgs.msg import Joy
|
||||||
from geometry_msgs.msg import TwistStamped
|
from geometry_msgs.msg import TwistStamped
|
||||||
from moveit_msgs.msg import PlanningScene
|
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
|
||||||
|
|
||||||
from enum import Enum
|
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.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.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')
|
self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
|
||||||
# Check if the a service is available
|
# Check if the a service is available
|
||||||
|
@ -62,6 +61,12 @@ class JoyMove(Node):
|
||||||
req = Trigger.Request()
|
req = Trigger.Request()
|
||||||
self.servo_client.call_async(req)
|
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):
|
def _joy_cb(self, msg):
|
||||||
axes = msg.axes
|
axes = msg.axes
|
||||||
|
@ -84,6 +89,7 @@ class JoyMove(Node):
|
||||||
ts.twist.angular.z = float(roll_positive + roll_negative)
|
ts.twist.angular.z = float(roll_positive + roll_negative)
|
||||||
|
|
||||||
ts.header.stamp = self.get_clock().now().to_msg()
|
ts.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
ts.header.frame_id = "base_link"
|
||||||
|
|
||||||
self.twist_pub.publish(ts)
|
self.twist_pub.publish(ts)
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue