Bkyb
commented
3 months ago It is not a solution to this problem, but I modified "dsr_hw_interface2.h" and "dsr_hw_interface2.cpp" to publish the desired value.
dsr_hw_interface2.h
...
unsigned int m_port;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_joint_state_pub_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr here_CurrentRotm; // I changed here
...dsr_hw_interface2.cpp
...
auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
m_joint_state_pub_ = m_node_->create_publisher<sensor_msgs::msg::JointState>("joint_states", qos);
here_CurrentRotm = m_node_->create_publisher<std_msgs::msg::Float64MultiArray>("msg/current_rotm", 10); // I changed here
...
...
/* I changed here */
auto heremsg = std_msgs::msg::Float64MultiArray();
heremsg.layout.dim.resize(2);
heremsg.layout.dim[0].label = "rows";
heremsg.layout.dim[0].size = 3;
heremsg.layout.dim[0].stride = 9;
heremsg.layout.dim[1].label = "cols";
heremsg.layout.dim[1].size = 3;
heremsg.layout.dim[1].stride = 3;
heremsg.data.resize(9);
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
if(pData){
g_stDrState.fRotationMatrix[j][i] = pData->_tCtrl._tTask._fRotationMatrix[j][i]; // Rotation Matrix
heremsg.data[i * 3 + j] = pData->_tCtrl._tTask._fRotationMatrix[j][i]; //
}
}
}
here_CurrentRotm -> publish(heremsg);
...Result
ros2 topic echo /dsr01/msg/current_rotm
layout:
dim:
- label: rows
size: 3
stride: 9
- label: cols
size: 3
stride: 3
data_offset: 0
data:
- -0.001293396227993071
- -0.9999966621398926
- 0.0022363809403032064
- -0.9999990463256836
- 0.001292342902161181
- -0.00047234821249730885
- 0.00046945648500695825
- -0.0022369897924363613
- -0.9999973773956299
---
I'm using Ubuntu 22.04 humble-devel version. When I request 'aux_control service', ALL values are ""ZERO"". How can I solve it?
Other requests are executed without problems.
Log