Compare commits
4 commits
Author | SHA1 | Date | |
---|---|---|---|
e401bd00fa | |||
3db39b9ec3 | |||
056fa3de68 | |||
8527b2411e |
11 changed files with 275 additions and 70 deletions
|
@ -2,7 +2,6 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll
|
||||||
moveit_simple_controller_manager:
|
moveit_simple_controller_manager:
|
||||||
controller_names:
|
controller_names:
|
||||||
- joint_trajectory_controller
|
- joint_trajectory_controller
|
||||||
|
|
||||||
joint_trajectory_controller:
|
joint_trajectory_controller:
|
||||||
action_ns: follow_joint_trajectory
|
action_ns: follow_joint_trajectory
|
||||||
type: FollowJointTrajectory
|
type: FollowJointTrajectory
|
||||||
|
|
69
src/ar3_config/config/ar3_joy.yaml
Normal file
69
src/ar3_config/config/ar3_joy.yaml
Normal file
|
@ -0,0 +1,69 @@
|
||||||
|
###############################################
|
||||||
|
# Modify all parameters related to servoing here
|
||||||
|
###############################################
|
||||||
|
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
|
||||||
|
|
||||||
|
## Properties of incoming commands
|
||||||
|
command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
|
||||||
|
scale:
|
||||||
|
# Scale parameters are only used if command_in_type=="unitless"
|
||||||
|
linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
|
||||||
|
rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
|
||||||
|
# Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
|
||||||
|
joint: 0.5
|
||||||
|
|
||||||
|
## Properties of outgoing commands
|
||||||
|
publish_period: 0.034 # 1/Nominal publish rate [seconds]
|
||||||
|
low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
|
||||||
|
|
||||||
|
# What type of topic does your robot driver expect?
|
||||||
|
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
|
||||||
|
command_out_type: trajectory_msgs/JointTrajectory
|
||||||
|
|
||||||
|
# What to publish? Can save some bandwidth as most robots only require positions or velocities
|
||||||
|
publish_joint_positions: true
|
||||||
|
publish_joint_velocities: false
|
||||||
|
publish_joint_accelerations: false
|
||||||
|
|
||||||
|
## Incoming Joint State properties
|
||||||
|
low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less.
|
||||||
|
|
||||||
|
## MoveIt properties
|
||||||
|
move_group_name: ar3_arm # Often 'manipulator' or 'arm'
|
||||||
|
planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'
|
||||||
|
|
||||||
|
## Other frames
|
||||||
|
ee_frame_name: link_6 # The name of the end effector link, used to return the EE pose
|
||||||
|
robot_link_command_frame: link_6 # 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
|
||||||
|
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
|
||||||
|
# Important because ROS may drop some messages and we need the robot to halt reliably.
|
||||||
|
num_outgoing_halt_msgs_to_publish: 4
|
||||||
|
|
||||||
|
## Configure handling of singularities and joint limits
|
||||||
|
lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
|
||||||
|
hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
|
||||||
|
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
|
||||||
|
|
||||||
|
## Topic names
|
||||||
|
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
|
||||||
|
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
|
||||||
|
joint_topic: /joint_states
|
||||||
|
status_topic: ~/status # Publish status to this topic
|
||||||
|
command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
|
||||||
|
|
||||||
|
## Collision checking for the entire robot body
|
||||||
|
check_collisions: true # Check collisions?
|
||||||
|
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
|
||||||
|
# Two collision check algorithms are available:
|
||||||
|
# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
|
||||||
|
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
|
||||||
|
collision_check_type: threshold_distance
|
||||||
|
# Parameters for "threshold_distance"-type collision checking
|
||||||
|
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
|
||||||
|
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
|
||||||
|
# Parameters for "stop_distance"-type collision checking
|
||||||
|
collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
|
||||||
|
min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]
|
|
@ -1,40 +0,0 @@
|
||||||
joint_limits:
|
|
||||||
joint1:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 40.1750
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 50.75
|
|
||||||
joint2:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 40.1750
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 30.875
|
|
||||||
joint3:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 40.1750
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 40.5
|
|
||||||
joint4:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 40.1750
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 50.125
|
|
||||||
joint5:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 40.6100
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 50.75
|
|
||||||
joint6:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 40.6100
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 70.0
|
|
||||||
|
|
||||||
cartesian_limits:
|
|
||||||
max_trans_vel: 10
|
|
||||||
max_trans_acc: 20.25
|
|
||||||
max_trans_dec: -50
|
|
||||||
max_rot_vel: 10.57
|
|
||||||
|
|
||||||
default_acceleration_scaling_factor: 1.0
|
|
||||||
default_velocity_scaling_factor: 1.0
|
|
|
@ -153,3 +153,4 @@ move_group:
|
||||||
- LazyPRMstarkConfigDefault
|
- LazyPRMstarkConfigDefault
|
||||||
- SPARSkConfigDefault
|
- SPARSkConfigDefault
|
||||||
- SPARStwokConfigDefault
|
- SPARStwokConfigDefault
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
@ -30,21 +31,58 @@ def generate_launch_description():
|
||||||
executable='rviz2',
|
executable='rviz2',
|
||||||
output='log',
|
output='log',
|
||||||
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
|
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
|
||||||
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]),
|
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
|
||||||
|
),
|
||||||
|
|
||||||
# Attatch robot to map
|
# Attatch robot to map
|
||||||
Node(
|
Node(
|
||||||
package='tf2_ros',
|
package='tf2_ros',
|
||||||
executable='static_transform_publisher',
|
executable='static_transform_publisher',
|
||||||
output='screen',
|
output='screen',
|
||||||
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']),
|
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
|
||||||
|
),
|
||||||
|
|
||||||
|
Node(
|
||||||
|
package='joystickmove',
|
||||||
|
executable='move',
|
||||||
|
output='screen',
|
||||||
|
),
|
||||||
|
|
||||||
|
# Create joystick controller
|
||||||
|
ComposableNodeContainer(
|
||||||
|
name="joystick_container",
|
||||||
|
namespace="/",
|
||||||
|
package="rclcpp_components",
|
||||||
|
executable="component_container",
|
||||||
|
composable_node_descriptions=[
|
||||||
|
ComposableNode(
|
||||||
|
package="moveit_servo",
|
||||||
|
plugin="moveit_servo::ServoServer",
|
||||||
|
name="servo_server",
|
||||||
|
parameters=[
|
||||||
|
{'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
],
|
||||||
|
extra_arguments=[{"use_intra_process_comms": True}],
|
||||||
|
),
|
||||||
|
ComposableNode(
|
||||||
|
package="joy",
|
||||||
|
plugin="joy::Joy",
|
||||||
|
name="joy_node",
|
||||||
|
extra_arguments=[{"use_intra_process_comms": True}],
|
||||||
|
)
|
||||||
|
],
|
||||||
|
output="screen",
|
||||||
|
),
|
||||||
|
|
||||||
# Controller
|
# Controller
|
||||||
Node(
|
Node(
|
||||||
package='controller_manager',
|
package='controller_manager',
|
||||||
executable='ros2_control_node',
|
executable='ros2_control_node',
|
||||||
output='screen',
|
output='screen',
|
||||||
parameters=[robot_description, join(config_root, 'controller_manager.yaml')]),
|
parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
|
||||||
|
),
|
||||||
|
|
||||||
# Start controllers
|
# Start controllers
|
||||||
ExecuteProcess(
|
ExecuteProcess(
|
||||||
|
@ -76,7 +114,6 @@ def generate_launch_description():
|
||||||
get_yaml(join(config_root, 'trajectory.yaml')),
|
get_yaml(join(config_root, 'trajectory.yaml')),
|
||||||
get_yaml(join(config_root, 'planning_scene.yaml')),
|
get_yaml(join(config_root, 'planning_scene.yaml')),
|
||||||
get_yaml(join(config_root, "ar3_controllers.yaml")),
|
get_yaml(join(config_root, "ar3_controllers.yaml")),
|
||||||
get_yaml(join(config_root, "joint_limits.yaml")),
|
|
||||||
],
|
],
|
||||||
),
|
),
|
||||||
|
|
||||||
|
@ -86,12 +123,4 @@ def generate_launch_description():
|
||||||
executable='robot_state_publisher',
|
executable='robot_state_publisher',
|
||||||
output='screen',
|
output='screen',
|
||||||
parameters=[robot_description]),
|
parameters=[robot_description]),
|
||||||
|
|
||||||
# Fake robot state publisher
|
|
||||||
# Node(
|
|
||||||
# package='ar3_description',
|
|
||||||
# executable='state_publisher',
|
|
||||||
# name='state_publisher',
|
|
||||||
# emulate_tty=True,
|
|
||||||
# output='screen'),
|
|
||||||
])
|
])
|
||||||
|
|
|
@ -92,7 +92,6 @@
|
||||||
link="link_1" />
|
link="link_1" />
|
||||||
<axis
|
<axis
|
||||||
xyz="0 0 1" />
|
xyz="0 0 1" />
|
||||||
<limit effort="0" velocity="-20.0" lower="-3.14" upper="3.14" />
|
|
||||||
</joint>
|
</joint>
|
||||||
<link
|
<link
|
||||||
name="link_2">
|
name="link_2">
|
||||||
|
@ -146,7 +145,6 @@
|
||||||
link="link_2" />
|
link="link_2" />
|
||||||
<axis
|
<axis
|
||||||
xyz="0 0 -1" />
|
xyz="0 0 -1" />
|
||||||
<limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
|
|
||||||
</joint>
|
</joint>
|
||||||
<link
|
<link
|
||||||
name="link_3">
|
name="link_3">
|
||||||
|
@ -200,7 +198,6 @@
|
||||||
link="link_3" />
|
link="link_3" />
|
||||||
<axis
|
<axis
|
||||||
xyz="0 0 -1" />
|
xyz="0 0 -1" />
|
||||||
<limit effort="0" velocity="200.0" lower="-3.14" upper="3.14" />
|
|
||||||
</joint>
|
</joint>
|
||||||
<link
|
<link
|
||||||
name="link_4">
|
name="link_4">
|
||||||
|
@ -254,7 +251,6 @@
|
||||||
link="link_4" />
|
link="link_4" />
|
||||||
<axis
|
<axis
|
||||||
xyz="0 0 -1" />
|
xyz="0 0 -1" />
|
||||||
<limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
|
|
||||||
</joint>
|
</joint>
|
||||||
<link
|
<link
|
||||||
name="link_5">
|
name="link_5">
|
||||||
|
@ -308,7 +304,6 @@
|
||||||
link="link_5" />
|
link="link_5" />
|
||||||
<axis
|
<axis
|
||||||
xyz="-1 0 0" />
|
xyz="-1 0 0" />
|
||||||
<limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
|
|
||||||
</joint>
|
</joint>
|
||||||
<link
|
<link
|
||||||
name="link_6">
|
name="link_6">
|
||||||
|
@ -362,7 +357,6 @@
|
||||||
link="link_6" />
|
link="link_6" />
|
||||||
<axis
|
<axis
|
||||||
xyz="0 0 1" />
|
xyz="0 0 1" />
|
||||||
<limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
|
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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()
|
||||||
|
@ -46,11 +54,11 @@ class CobotMove(Node):
|
||||||
print(self._serial.readline())
|
print(self._serial.readline())
|
||||||
|
|
||||||
def _move_j1_cb(self, msg):
|
def _move_j1_cb(self, msg):
|
||||||
self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
|
self._cur[0] = int(msg.data / pi / 2. * 8000)
|
||||||
self._move_servo()
|
self._move_servo()
|
||||||
|
|
||||||
def _move_j2_cb(self, msg):
|
def _move_j2_cb(self, msg):
|
||||||
self._cur[1] = int(msg.data / pi / 2. * 10000 * 4)
|
self._cur[1] = int(msg.data / pi / 2. * 10000)
|
||||||
self._move_servo()
|
self._move_servo()
|
||||||
|
|
||||||
def _move_j3_cb(self, msg):
|
def _move_j3_cb(self, msg):
|
||||||
|
@ -74,11 +82,10 @@ class CobotMove(Node):
|
||||||
|
|
||||||
|
|
||||||
line = self._serial.readline().decode('UTF-8')
|
line = self._serial.readline().decode('UTF-8')
|
||||||
# print(line)
|
|
||||||
|
|
||||||
blocks = re.findall(r'[ABCDEF]-*\d+', line)
|
blocks = re.findall(r'[ABCDEF]-*\d+', line)
|
||||||
a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
|
a = float(blocks[0][1:]) / 8000. * pi * 2.
|
||||||
b = float(blocks[1][1:]) / 10000. / 4. * 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.
|
||||||
|
@ -90,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):
|
||||||
|
@ -106,3 +119,4 @@ def main(args=None):
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
main()
|
main()
|
||||||
|
|
||||||
|
|
|
@ -1,8 +1,40 @@
|
||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
|
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 moveit_msgs.msg import PlanningScene, PlanningSceneWorld
|
||||||
|
from control_msgs.msg import JointJog
|
||||||
|
|
||||||
|
from enum import Enum
|
||||||
|
|
||||||
|
class Buttons(Enum):
|
||||||
|
CROSS = 0
|
||||||
|
CIRCLE = 1
|
||||||
|
TRIANGLE = 2
|
||||||
|
SQUARE = 3
|
||||||
|
L1 = 4
|
||||||
|
R1 = 5
|
||||||
|
L2 = 6
|
||||||
|
R2 = 7
|
||||||
|
SELECT = 8
|
||||||
|
START = 9
|
||||||
|
CENTER = 10
|
||||||
|
L3 = 11
|
||||||
|
R3 = 12
|
||||||
|
|
||||||
|
class Axes(Enum):
|
||||||
|
LEFT_X = 0
|
||||||
|
LEFT_Y = 1
|
||||||
|
L1 = 2
|
||||||
|
RIGHT_X = 3
|
||||||
|
RIGHT_Y = 4
|
||||||
|
R1 = 5
|
||||||
|
DPAD_UPDOWN = 6
|
||||||
|
DPAD_LEFTRIGHT = 7
|
||||||
|
|
||||||
class JoyMove(Node):
|
class JoyMove(Node):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
@ -10,13 +42,73 @@ class JoyMove(Node):
|
||||||
self._logger = self.get_logger()
|
self._logger = self.get_logger()
|
||||||
self._logger.info('joystick init')
|
self._logger.info('joystick init')
|
||||||
|
|
||||||
|
JOY_TOPIC = '/joy'
|
||||||
|
TWIST_TOPIC = '/servo_server/delta_twist_cmds'
|
||||||
|
JOINT_TOPIC = '/servo_server/delta_joint_cmds'
|
||||||
|
EE = 'joint_6'
|
||||||
|
BASE = 'base_link'
|
||||||
|
|
||||||
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, '/move/j1', 10)
|
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);
|
||||||
|
|
||||||
|
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.')
|
||||||
|
|
||||||
|
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):
|
def _joy_cb(self, msg):
|
||||||
asd = msg.axes[1]
|
axes = msg.axes
|
||||||
self.base_pub.publish(Float32(data=asd))
|
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]:
|
||||||
|
|
||||||
|
joint = JointJog()
|
||||||
|
|
||||||
|
joint.joint_names = ['joint_1', 'joint_2',
|
||||||
|
'joint_6', 'joint_5']
|
||||||
|
joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]),
|
||||||
|
float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
|
||||||
|
joint.duration = 1.0
|
||||||
|
|
||||||
|
joint.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
joint.header.frame_id = "link_3"
|
||||||
|
self.joint_pub.publish(joint)
|
||||||
|
return
|
||||||
|
|
||||||
|
ts = TwistStamped()
|
||||||
|
|
||||||
|
ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
|
||||||
|
ts.twist.linear.y = axes[Axes.RIGHT_X.value]
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
ts.twist.angular.y = axes[Axes.LEFT_Y.value]
|
||||||
|
ts.twist.angular.x = axes[Axes.LEFT_X.value]
|
||||||
|
|
||||||
|
roll_positive = buttons[Buttons.R2.value]
|
||||||
|
roll_negative = -1 * buttons[Buttons.L2.value]
|
||||||
|
ts.twist.angular.z = float(roll_positive + roll_negative)
|
||||||
|
|
||||||
|
ts.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
ts.header.frame_id = "link_6"
|
||||||
|
|
||||||
|
self.twist_pub.publish(ts)
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
|
|
|
@ -20,7 +20,7 @@ setup(
|
||||||
tests_require=['pytest'],
|
tests_require=['pytest'],
|
||||||
entry_points={
|
entry_points={
|
||||||
'console_scripts': [
|
'console_scripts': [
|
||||||
'joystickmove = joystickmove.move:main'
|
'move = joystickmove.move:main'
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|
Loading…
Reference in a new issue