Skip to content

Commit 5c4342a

Browse files
RoozkiRoozki
committed
Fuckit we merge
1 parent 1e07d62 commit 5c4342a

4 files changed

Lines changed: 102 additions & 70 deletions

File tree

‎src/arm_control/include/cbs_interface.h‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,18 @@ class CBSArmInterface : public rclcpp::Node
1212
public:
1313
CBSArmInterface() : Node("CBSArmInterface"){
1414
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
15-
arm_cmd_publisher = this->create_publisher<rover_msgs::msg::ArmCommand>("/arm/command", qos);
15+
// arm_cmd_publisher = this->create_publisher<rover_msgs::msg::ArmCommand>("/arm/command", qos);
1616

1717
arm_panel_subscriber = this->create_subscription<rover_msgs::msg::ArmPanel>(
1818
"/cbs/arm_panel", 10, std::bind(&CBSArmInterface::arm_panel_callback, this, std::placeholders::_1));
1919
left_panel_subscriber = this->create_subscription<rover_msgs::msg::GenericPanel>(
2020
"/cbs/left_panel_a", 10, std::bind(&CBSArmInterface::left_panel_callback, this, std::placeholders::_1));
2121

22-
arm_ik_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
23-
"/arm_moveit_control/delta_twist_cmds", qos);
22+
// arm_ik_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
23+
// "/arm_moveit_control/delta_twist_cmds", qos);
24+
ptz_pub = this->create_publisher<geometry_msgs::msg::Vector3>(
25+
"/ptz/control", qos);
26+
2427
// arm_panel_timer = this->create_wall_timer( //Timer setup if we need it
2528
// std::chrono::milliseconds(10), // Timer interval
2629
// std::bind(&CBSManagerNode::armPanelPoll, this) // Callback function
@@ -32,10 +35,12 @@ class CBSArmInterface : public rclcpp::Node
3235
~CBSArmInterface(){
3336
RCLCPP_WARN(this->get_logger(), "WARNING: ARM PANEL INTERFACE NODE OFFLINE!");
3437
}
35-
rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_cmd_publisher;
38+
// rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_cmd_publisher;
3639
rclcpp::Subscription<rover_msgs::msg::ArmPanel>::SharedPtr arm_panel_subscriber;
3740
rclcpp::Subscription<rover_msgs::msg::GenericPanel>::SharedPtr left_panel_subscriber;
38-
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr arm_ik_pub;
41+
// rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr arm_ik_pub;
42+
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr ptz_pub;
43+
3944

4045
// rclcpp::Publisher<
4146
private:

‎src/arm_control/src/cbs_interface.cpp‎

Lines changed: 84 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -1,75 +1,98 @@
11
//* This node will take the cbs topic for the arm panel and send things around
22
#include "cbs_interface.h"
3-
void CBSArmInterface::arm_panel_callback(const rover_msgs::msg::ArmPanel::SharedPtr msg){
4-
if(ik){
5-
geometry_msgs::msg::TwistStamped ik_msg;
6-
7-
ik_msg.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();
8-
ik_msg.header.frame_id = "link_tt";
9-
// ik_msg.twist.linear.x = static_cast<float>(msg->
10-
ik_msg.twist.linear.x = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2 / 180;
11-
ik_msg.twist.linear.y = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1 / 180;
12-
ik_msg.twist.linear.z = (static_cast<float>(msg->right.x) - 50)/100 * max_joysticks_output_speed_deg[4]*2 / 180;
13-
ik_msg.twist.angular.x = (static_cast<float>(msg->right.z) - 50)/100 * max_joysticks_output_speed_deg[5]*2 / 180;
14-
ik_msg.twist.angular.y = (static_cast<float>(msg->right.y) - 50)/100 * max_joysticks_output_speed_deg[2]*2*-1 / 180;
15-
ik_msg.twist.angular.z = (static_cast<float>(msg->left.z) - 50)/100 * max_joysticks_output_speed_deg[3]*2 / 180;
16-
// float value = ik_hmi_speed[index]; ///TODO get speed based on spinbuttons
17-
// switch (index)
18-
// {
19-
// case 0: // Linear X
20-
// ik_msg.twist.linear.x = value;
21-
// break;
22-
// case 1: // Linear Y
23-
// ik_msg.twist.linear.y = value;
24-
// break;
25-
// case 2: // Linear Z
26-
// ik_msg.twist.linear.z = value;
27-
// break;
28-
// case 3: // Angular X
29-
// ik_msg.twist.angular.x = value;
30-
// break;
31-
// case 4: // Angular Y
32-
// ik_msg.twist.angular.y = value;
33-
// break;
34-
// case 5: // Angular Z
35-
// ik_msg.twist.angular.z = value;
36-
// break;
37-
// default:
38-
// RCLCPP_WARN(this->get_logger(), "Invalid index: %d. Must be 0-5.", index);
39-
// return;
40-
// }
3+
// void CBSArmInterface::arm_panel_callback(const rover_msgs::msg::ArmPanel::SharedPtr msg){
4+
// if(ik){
5+
// geometry_msgs::msg::TwistStamped ik_msg;
6+
7+
// ik_msg.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();
8+
// ik_msg.header.frame_id = "link_tt";
9+
// // ik_msg.twist.linear.x = static_cast<float>(msg->
10+
// ik_msg.twist.linear.x = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2 / 180;
11+
// ik_msg.twist.linear.y = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1 / 180;
12+
// ik_msg.twist.linear.z = (static_cast<float>(msg->right.x) - 50)/100 * max_joysticks_output_speed_deg[4]*2 / 180;
13+
// ik_msg.twist.angular.x = (static_cast<float>(msg->right.z) - 50)/100 * max_joysticks_output_speed_deg[5]*2 / 180;
14+
// ik_msg.twist.angular.y = (static_cast<float>(msg->right.y) - 50)/100 * max_joysticks_output_speed_deg[2]*2*-1 / 180;
15+
// ik_msg.twist.angular.z = (static_cast<float>(msg->left.z) - 50)/100 * max_joysticks_output_speed_deg[3]*2 / 180;
16+
// // float value = ik_hmi_speed[index]; ///TODO get speed based on spinbuttons
17+
// // switch (index)
18+
// // {
19+
// // case 0: // Linear X
20+
// // ik_msg.twist.linear.x = value;
21+
// // break;
22+
// // case 1: // Linear Y
23+
// // ik_msg.twist.linear.y = value;
24+
// // break;
25+
// // case 2: // Linear Z
26+
// // ik_msg.twist.linear.z = value;
27+
// // break;
28+
// // case 3: // Angular X
29+
// // ik_msg.twist.angular.x = value;
30+
// // break;
31+
// // case 4: // Angular Y
32+
// // ik_msg.twist.angular.y = value;
33+
// // break;
34+
// // case 5: // Angular Z
35+
// // ik_msg.twist.angular.z = value;
36+
// // break;
37+
// // default:
38+
// // RCLCPP_WARN(this->get_logger(), "Invalid index: %d. Must be 0-5.", index);
39+
// // return;
40+
// // }
4141

42-
arm_ik_pub->publish(ik_msg);
43-
}else{
42+
// arm_ik_pub->publish(ik_msg);
43+
// }else{
4444

4545

4646

47-
rover_msgs::msg::ArmCommand cmd_msg;
48-
cmd_msg.cmd_type = 'V'; //!SHOULD BE FROM ArmSerialProtocol.h
49-
cmd_msg.velocities.resize(NUM_JOINTS);
50-
cmd_msg.velocities[0] = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2;
51-
cmd_msg.velocities[1] = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1;
52-
cmd_msg.velocities[2] = (static_cast<float>(msg->right.y) - 50)/100 * max_joysticks_output_speed_deg[2]*2*-1;
53-
cmd_msg.velocities[3] = (static_cast<float>(msg->left.z) - 50)/100 * max_joysticks_output_speed_deg[3]*2;
54-
cmd_msg.velocities[4] = (static_cast<float>(msg->right.x) - 50)/100 * max_joysticks_output_speed_deg[4]*2;
55-
cmd_msg.velocities[5] = (static_cast<float>(msg->right.z) - 50)/100 * max_joysticks_output_speed_deg[5]*2;
47+
// rover_msgs::msg::ArmCommand cmd_msg;
48+
// cmd_msg.cmd_type = 'V'; //!SHOULD BE FROM ArmSerialProtocol.h
49+
// cmd_msg.velocities.resize(NUM_JOINTS);
50+
// cmd_msg.velocities[0] = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2;
51+
// cmd_msg.velocities[1] = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1;
52+
// cmd_msg.velocities[2] = (static_cast<float>(msg->right.y) - 50)/100 * max_joysticks_output_speed_deg[2]*2*-1;
53+
// cmd_msg.velocities[3] = (static_cast<float>(msg->left.z) - 50)/100 * max_joysticks_output_speed_deg[3]*2;
54+
// cmd_msg.velocities[4] = (static_cast<float>(msg->right.x) - 50)/100 * max_joysticks_output_speed_deg[4]*2;
55+
// cmd_msg.velocities[5] = (static_cast<float>(msg->right.z) - 50)/100 * max_joysticks_output_speed_deg[5]*2;
5656

57-
if(msg->left.button && msg->right.button){
57+
// if(msg->left.button && msg->right.button){
5858

59-
cmd_msg.end_effector = 0;
60-
}else{
61-
if(msg->left.button){
59+
// cmd_msg.end_effector = 0;
60+
// }else{
61+
// if(msg->left.button){
6262

63-
cmd_msg.end_effector = -50;//-0.07;
64-
}
65-
if(msg->right.button){
63+
// cmd_msg.end_effector = -50;//-0.07;
64+
// }
65+
// if(msg->right.button){
6666

67-
cmd_msg.end_effector = 50;//0.07;
68-
}
69-
}
67+
// cmd_msg.end_effector = 50;//0.07;
68+
// }
69+
// }
70+
71+
// arm_cmd_publisher->publish(cmd_msg);
72+
// }
73+
// }
74+
75+
void CBSArmInterface::arm_panel_callback(const rover_msgs::msg::ArmPanel::SharedPtr msg){
76+
geometry_msgs::msg::Vector3 outmsg;
77+
float DEADZONE = 4.0;
78+
79+
80+
float x = msg->right.x - 50;
81+
float y = msg->right.y - 50;
82+
83+
if(x <= DEADZONE && x >= -DEADZONE)
84+
{
85+
x = 0;
86+
}
87+
if(y <= DEADZONE && y >= -DEADZONE)
88+
{
89+
y = 0;
90+
}
91+
92+
outmsg.x = x;
93+
outmsg.y = y;
94+
ptz_pub->publish(outmsg);
7095

71-
arm_cmd_publisher->publish(cmd_msg);
72-
}
7396
}
7497

7598
void CBSArmInterface::left_panel_callback(const rover_msgs::msg::GenericPanel::SharedPtr msg){

‎src/arm_hardware_interface/include/ptz_translator.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ class PtzControlNode : public rclcpp::Node
3131
std::string control_topic;
3232
std::string outgoing_topic;
3333
uint16_t device_id = 0;
34-
double max_speed = 100.0; // servo set_speed() units (percent)
34+
double max_speed = 100.0;
3535
double deadband = 0.0; // inputs below this are treated as 0
3636
bool invert_pan = false;
3737
bool invert_tilt = false;

‎src/drive_control/src/drive_control.cpp‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,13 @@ DriveControlNode::DriveControlNode() : Node("drive_control_node") {
1515

1616
void DriveControlNode::joyCallback(const sensor_msgs::msg::Joy::SharedPtr msg) {
1717
geometry_msgs::msg::Twist twist_msg;
18-
19-
twist_msg.linear.x = msg->axes[1] * MAX_LINEAR_SPEED_MPS;
20-
twist_msg.angular.z = msg->axes[0] * -MAX_ANGULAR_SPEED_MPS;
18+
float linear_speed = MAX_LINEAR_SPEED_MPS;
19+
if(msg->buttons[0])
20+
{
21+
linear_speed = linear_speed *1.5;
22+
}
23+
twist_msg.linear.x = msg->axes[1] * linear_speed * -1;
24+
twist_msg.angular.z = msg->axes[3] * -MAX_ANGULAR_SPEED_MPS;
2125

2226
cmd_vel_pub_->publish(twist_msg);
2327
}

0 commit comments

Comments
 (0)