ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Using Int64MultiArray msg for subscriber in micro_ros

asked 2021-08-10 05:44:24 -0600

Sarwan gravatar image

updated 2021-08-10 06:07:41 -0600

gvdhoorn gravatar image

Hi,

I have been trying to control the dynamixel using the data received by the subscriber of msg type Int64MultiArray. I do not know what the problem is, I could not able to make it work. I could able to publish the array but not subscribe. When the msg type of subscriber is changed to Int32 it is working fine but I need to get a array of data( six values).

Kindly inform what is the problem or any alternate methods.

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void subscription_callback(const void * msgin)
{ 

  const std_msgs__msg__Int64MultiArray * goal_vel = (const std_msgs__msg__Int64MultiArray *)msgin;
  int64_t left_front; 

    value[0] = int64_t(goal_vel->data.data[0]);
    value[1] = goal_vel->data.data[1];
    left_front = (goal_vel->data.data[0]);

    //left_front = int32_t(msg->data);
    dxl.setGoalVelocity(DXL_ID,value[0]);

   }

void setup() {
  // put your setup code here, to run once:
    set_microros_transports();

    allocator = rcl_get_default_allocator();

    //create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    //create node 
    RCCHECK(rclc_node_init_default(&node, "dynamixel_control", "",&support));

    //create publisher
    RCCHECK(rclc_publisher_init_default(&publisher, &node, 
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64MultiArray), "ticks"));

     // create subscriber
    RCCHECK(rclc_subscription_init_default(&subscriber,&node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64MultiArray),  "drive_cmd"));

    // create executor
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));
    RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &goal_vel, &subscription_callback, ON_NEW_DATA));

      Serial.begin(115200);
    dxl.begin(1000000);
    dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

    //dxl.ping(DXL_ID);
    dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_VELOCITY);
  dxl.torqueOn(DXL_ID);
  dxl.setGoalVelocity(DXL_ID,20);

  goal_vel = std_msgs__msg__Int64MultiArray__create();
  ticks = std_msgs__msg__Int64MultiArray__create();

  goal_vel->data.data = (int64_t*)malloc(sizeof(value));
  ticks->data.data = (int64_t*)malloc(sizeof(array_tic)); 

  ticks->data.size = sizeof(array_tic)/sizeof(int64_t);memcpy(ticks->data.data,array_tic,sizeof(array_tic));
  goal_vel->data.size = sizeof(value)/sizeof(int64_t);memcpy(goal_vel->data.data, value,sizeof(value));
}

void loop() {
  // put your main code here, to run repeatedly:
    delay(100);
    array_tic[0]= left_tick;
    array_tic[1] = right_tick;
    left_tick =  dxl.getPresentPosition(DXL_ID);  
    right_tick =  dxl.getPresentPosition(2); 
    ticks->data.data = array_tic;  

    RCSOFTCHECK(rcl_publish(&publisher, ticks, NULL));
    RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2021-08-10 05:57:26 -0600

Pablogs gravatar image

The problem is the initialization of goal_vel. In micro-ROS you should not rely on default init functions. Please read this tutorial to learn how to init message memory correctly.

If your problem is not solved, please open an issue in our Github repos.

edit flag offensive delete link more

Comments

I read the document and initialized like this. but still it is not working

int64_t array_tic[2]= {left_tick,right_tick}; int64_t value[10] = {0,0,0,0,0,0,0,0,0,0};

goal_vel = std_msgs__msg__Int64MultiArray__create(); goal_vel->data.capacity = 10; goal_vel->data.data = (int64_t*)malloc(goal_vel->data.capacity * sizeof(int64_t)); goal_vel->data.size = sizeof(value)/sizeof(int64_t);memcpy(goal_vel->data.data, value,sizeof(value)); //

  ticks = std_msgs__msg__Int64MultiArray__create();  
  ticks->data.capacity = 2;
  ticks->data.data = (int64_t*)malloc(sizeof(array_tic));   
  ticks->data.size = sizeof(array_tic)/sizeof(int64_t);memcpy(ticks->data.data,array_tic,sizeof(array_tic));
Sarwan gravatar image Sarwan  ( 2021-08-10 08:37:45 -0600 )edit

In the document does not appear any std_msgs__msg__Int64MultiArray__create function.

Also, it is stated that you have to check all members of your type recursively, you are not initializing layout member, nor any of its sub-members.

Pablogs gravatar image Pablogs  ( 2021-08-11 01:37:18 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2021-08-10 05:44:24 -0600

Seen: 418 times

Last updated: Aug 10 '21